diff --git a/experimental/beacon_sim/analysis/analysis.py b/experimental/beacon_sim/analysis/analysis.py index 4c795387..c1772303 100755 --- a/experimental/beacon_sim/analysis/analysis.py +++ b/experimental/beacon_sim/analysis/analysis.py @@ -18,8 +18,11 @@ np.set_printoptions(linewidth=200) -def add_xy_to_axes(x, y, ax, name='', xlabel='', ylabel='', use_log=False): - ax.semilogy(x, y, marker='o', label=name) +def add_xy_to_axes(x, y, ax, name='', xlabel='', ylabel='', use_log=True): + if use_log: + ax.semilogy(x, y, marker='o', label=name) + else: + ax.plot(x, y, marker='o', label=name) ax.set_xlabel(xlabel) ax.set_ylabel(ylabel) ax.grid(True) @@ -185,6 +188,82 @@ def sens( print('||P_ekf - P_tf|| =', diff_ekf_tf) print('||P_scatter - P_expected|| =', diff_expected) +@main.command(name="back", help='Test backwards belief update.') +@click.argument("model", default="si") +@click.option("--num-props", type=int, default=500) +@click.option("--dt", type=float, default=0.1) +@click.option("--info", is_flag=True, type=bool, default=False, help='Use information filter') +def sens( + model, + num_props, #968, largest num props before fail for di + dt, + info +): + #setup model + if model=='si': + P = np.identity(2) + robot = SingleIntegrator(dt=dt) + elif model=='di': + P = np.identity(4) + robot = DoubleIntegrator(dt=dt) + else: + print('error. invalid model') + raise + + filter_type = FilterType.INFORMATION if info else FilterType.COVARIANCE + bp = BeliefPropagation(robot, filter_type) + + #print covariance matrices for + P_forward = bp.prop(P, k=num_props, backward=False) + P_backward = bp.prop(P_forward, k=num_props, backward=True) + print('\nP:',P) + print('\nP_forward from P:',P_forward) + print('\nP_backward from P_forward:',P_backward) + + #compute condition number for each num props up to + print('\ngenerating plot...') + data={} + data['initial']=defaultdict(list) + data['forward']=defaultdict(list) + data['backward']=defaultdict(list) + data['k']=defaultdict(list) + for k in range(num_props): + P_forward = bp.prop(P, k=k, backward=False) + P_backward = bp.prop(P_forward, k=k, backward=True) + data['initial']['cond'].append(LA.cond(P)) + data['initial']['det'].append(LA.det(P)) + data['initial']['k'].append(k) + data['forward']['cond'].append(LA.cond(P_forward)) + data['forward']['det'].append(LA.det(P_forward)) + data['forward']['k'].append(k) + data['backward']['cond'].append(LA.cond(P_backward)) + data['backward']['det'].append(LA.det(P_backward)) + data['backward']['k'].append(k) + + use_log=True + fig, axes = plt.subplots(nrows=2, ncols=2, figsize=(10, 5)) + + add_xy_to_axes(data['initial']['k'], data['initial']['cond'],axes[0,0], name='$\kappa(\Sigma_{initial})$', xlabel='number of steps', ylabel='cond',use_log=use_log) + add_xy_to_axes(data['forward']['k'], data['forward']['cond'],axes[0,0], name='$\kappa(\Sigma_{forward})$', xlabel='number of steps', ylabel='cond',use_log=use_log) + # axes[0,0].set_title('cond num forward',pad=20) + axes[0,0].legend(loc='upper left') + + add_xy_to_axes(data['initial']['k'], data['initial']['cond'],axes[0,1], name='$\kappa(\Sigma_{initial})$', xlabel='number of steps', ylabel='cond',use_log=use_log) + add_xy_to_axes(data['backward']['k'], data['backward']['cond'],axes[0,1], name='$\kappa(\Sigma_{backward})$', xlabel='number of steps', ylabel='cond',use_log=use_log) + # axes[0,1].set_title('cond num backward',pad=20) + axes[0,1].legend(loc='upper left') + + add_xy_to_axes(data['initial']['k'], data['initial']['det'],axes[1,0], name='$det(\Sigma_{initial})$', xlabel='number of steps', ylabel='det',use_log=use_log) + add_xy_to_axes(data['forward']['k'], data['forward']['det'],axes[1,0], name='$det(\Sigma_{forward})$', xlabel='number of steps', ylabel='det',use_log=use_log) + # axes[1,0].set_title('det forward',pad=20) + axes[1,0].legend(loc='upper left') + + add_xy_to_axes(data['initial']['k'], data['initial']['det'],axes[1,1], name='$det(\Sigma_{initial})$', xlabel='number of steps', ylabel='det',use_log=use_log) + add_xy_to_axes(data['backward']['k'], data['backward']['det'],axes[1,1], name='$det(\Sigma_{backward})$', xlabel='number of steps', ylabel='det',use_log=use_log) + # axes[1,1].set_title('det backward',pad=20) + axes[1,1].legend(loc='upper left') + + plt.show() if __name__ == "__main__": mpl.use("GTK3Agg") diff --git a/experimental/beacon_sim/analysis/belief_propagation.py b/experimental/beacon_sim/analysis/belief_propagation.py index 25921c57..441b41a2 100644 --- a/experimental/beacon_sim/analysis/belief_propagation.py +++ b/experimental/beacon_sim/analysis/belief_propagation.py @@ -25,46 +25,67 @@ def remap_vars(self): M = self.model.H().transpose() @ inv(self.model.R) @ self.model.H() return G, R, M - def predict_covariance(self,P): - P = self.model.F() @ P @ self.model.F().transpose() + self.model.L() @ self.model.Q @ self.model.L().transpose() + def predict_covariance(self,P,backward=False): + if not backward: + P = self.model.F() @ P @ self.model.F().transpose() + self.model.L() @ self.model.Q @ self.model.L().transpose() + else: + P = inv(self.model.F()) @ (P - self.model.L() @ self.model.Q @ self.model.L().transpose()) @ inv(self.model.F().transpose()) return P - def update_covariance(self,P): - n = P.shape[0] - S = self.model.H() @ P @ self.model.H().transpose() + self.model.M() @ self.model.R @ self.model.M().transpose() - K= P @ self.model.H().transpose() @ inv(S) - return (np.identity(n) - K @ self.model.H()) @ P + def update_covariance(self,P,backward=False): + if not backward: + n = P.shape[0] + S = self.model.H() @ P @ self.model.H().transpose() + self.model.M() @ self.model.R @ self.model.M().transpose() + K= P @ self.model.H().transpose() @ inv(S) + return (np.identity(n) - K @ self.model.H()) @ P + else: + n = P.shape[0] + Sbar = self.model.M() @ self.model.R @ self.model.M().transpose() - self.model.H() @ P @ self.model.H().transpose() + Kbar = P @ self.model.H().transpose() @ inv(Sbar) + return (np.identity(n) + Kbar @ self.model.H()) @ P - def predict_information(self, info): - return inv(self.predict_covariance(inv(info))) + def predict_information(self, info, backward=False): + return inv(self.predict_covariance(inv(info), backward=backward)) - def update_information(self, info): + def update_information(self, info, backward=False): meas_info = self.model.H().transpose() @ inv(self.model.R) @ self.model.H() - return info + meas_info + if not backward: + return info + meas_info + else: + return info - meas_info - def predict(self, state): - if self.filter_type == FilterType.Covariance: - return self.predict_covariance(state) + def predict(self, state, backward=False): + if self.filter_type == FilterType.COVARIANCE: + return self.predict_covariance(state, backward=backward) else: - return self.predict_information(state) + return self.predict_information(state, backward=backward) - def update(self, state): - if self.filter_type == FilterType.Covariance: - return self.update_covariance(state) + def update(self, state, backward=False): + if self.filter_type == FilterType.COVARIANCE: + return self.update_covariance(state, backward=backward) else: - return self.update_information(state) + return self.update_information(state, backward=backward) #functions for propagating belief - def prop(self, P, k=1): + def prop(self, P, k=1, backward=False): """ Propagates a belief P through k steps using EKF equations """ - for i in range(k): - P = self.predict(P) - P = self.update(P) + if not backward: + for i in range(k): + P = self.predict(P, backward=False) + P = self.update(P, backward=False) + else: + #combined equation for backward prop + #S_bar = R - HPH' + #K_bar = P * H' * S_bat^{-1} + #P0 = F^{-1} * [ (I + K_bar*H)*P - Q ] * F'^{-1}' + for i in range(k): + P = self.update(P, backward=True) + P = self.predict(P, backward=True) return P def prop_tf(self, P, k=1, only_predict=False, return_tf=False):