def computeJacobian(omegas_m, accs_m, biases, dt): """ Compute first-order Jacobian for bias update. Note: same for each distribution assuming Delta t is small. """ J = torch.zeros(9, 6) G = torch.zeros(9, 6) F = torch.eye(9) F[6:9, 3:6] = torch.eye(3) * dt G[:3, :3] = -dt * torch.eye(3) G[3:6, 3:6] = -dt * torch.eye(3) G[6:9, 3:6] = -0.5 * (dt**2) * torch.eye(3) Upsilon = torch.eye(5) Omegas = SO3.exp((omegas_m * dt).cuda()).cpu() invJac = SO3.inv_left_jacobian((omegas_m * dt).cuda()).cpu() for k in range(accs_m.shape[0]): acc = accs_m[k] Upsilon[:3, :3] = Omegas[k] Upsilon[:3, 3] = acc * dt Upsilon[:3, 4] = 1 / 2 * acc * (dt**2) G[:3, :3] = -invJac[k] * dt G[3:6, 3:6] = -dt * Upsilon[:3, :3].t() G[6:9, 3:6] = -0.5 * (dt**2) * Upsilon[:3, :3].t() J = SE3_2.uAd(Upsilon.inverse()).mm(F).mm(J) + G return J
def propagate(T0, P, Upsilon, Q, method, dt, g, cholQ=0): """Propagate state for one time step""" Gamma = f_Gamma(g, dt) Phi = f_flux(T0, dt) # propagate the mean T = Gamma.mm(Phi).mm(Upsilon) # Jacobian for propagating prior along time F = torch.eye(9) F[6:9, 3:6] = torch.eye(3) * dt # compute Adjoint of right transformation mean AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon)) Pprime = axat(AdUps.mm(F), P) # compound the covariances based on the second-order method Pprop = Pprime + Q if method == 1: # add fourth-order method Pprop += four_order(Pprime, Q) elif method == 2: # Monte Carlo method n_tot_samples = 1000000 nsamples = 50000 N = int(n_tot_samples / nsamples) + 1 tmp = torch.cholesky(P + 1e-20 * torch.eye(9)) cholP = tmp.cuda().expand(nsamples, 9, 9) cholQ = cholQ.cuda().expand(nsamples, 9, 9) Pprop = torch.zeros(9, 9) Gamma = Gamma.cuda().expand(nsamples, 5, 5) Upsilon = Upsilon.cuda().expand(nsamples, 5, 5) T0 = T0.cuda().expand(nsamples, 5, 5) T_inv = T.inverse().cuda().expand(nsamples, 5, 5) for i in range(N): xi0 = bmv(cholP, torch.randn(nsamples, 9).cuda()) w = bmv(cholQ, torch.randn(nsamples, 9).cuda()) T0_i = T0.bmm(SE3_2.exp(xi0)) Phi = f_flux(T0_i, dt) Upsilon_i = Upsilon.bmm(SE3_2.exp(w)) T_i = Gamma.bmm(Phi).bmm(Upsilon_i) xi = SE3_2.log(T_inv.bmm(T_i)) xi_mean = xi.mean(dim=0) Pprop += bouter(xi - xi_mean, xi - xi_mean).sum(dim=0).cpu() Pprop = Pprop / (N * nsamples + 1) Pprop = (Pprop + Pprop.t()) / 2 # symmetric return T, Pprop
def propagate(T0, Sigma, Upsilon, Q, method, dt, g): """Propagate state for one time step""" Gamma = f_Gamma(g, dt) Phi = f_flux(T0, dt) # propagate the mean T = Gamma.mm(Phi).mm(Upsilon) # Jacobian for propagating prior along time F = torch.eye(9) F[6:9, 3:6] = torch.eye(3) * dt # compute Adjoint of right transformation mean AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon)) Sigma_tmp = axat(AdUps.mm(F), Sigma) # compound the covariances based on the second-order method Sigma_prop = Sigma_tmp + Q if method == 1: # add fourth-order method Sigma_prop += four_order(Sigma_tmp, Q) elif method == 2: # SO(3) x R^6 wedge_acc = SO3.uwedge(Upsilon[:3, 3]) # already multiplied by dt F = torch.eye(9) F[3:6, :3] = T0[:3, :3].t() F[3:6, :3] = -T0[:3, :3].mm(wedge_acc) F[6:9, :3] = F[3:6, :3] * dt / 2 F[6:9, 3:6] = dt * torch.eye(3) G = torch.zeros(9, 6) G[:3, :3] = dt * T0[:3, :3].t() G[3:6, 3:6] = T0[:3, :3] * dt G[6:9, 3:6] = 1 / 2 * T0[:3, :3] * (dt**2) Sigma_prop = axat(F, Sigma) + axat(G, Q[:6, :6] / (dt**2)) Sigma_prop = (Sigma_prop + Sigma_prop.t()) / 2 # symmetric return T, Sigma_prop
def propagate(T0, Sigma, Upsilon, Q, method, dt, g, cholQ=0): """Propagate state for one time step""" Gamma = f_Gamma(g, dt) Phi = f_flux(T0, dt) # propagate the mean T = Gamma.mm(Phi).mm(Upsilon) # Jacobian for propagating prior along time F = torch.eye(9) F[6:9, 3:6] = torch.eye(3) * dt # compute Adjoint of right transformation mean AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon)) Sigma_tmp = axat(AdUps.mm(F), Sigma) # compound the covariances based on the second-order method Sigma_prop = Sigma_tmp + Q if method == 1: # add fourth-order method Sigma_prop += four_order(Sigma_tmp, Q) Sigma_prop = (Sigma_prop + Sigma_prop.t()) / 2 # symmetric return T, Sigma_prop
def compound(T0, Sigma, Upsilon, Q, method, dt, g, cholQ=0): Gamma = f_Gamma(g, dt) Phi = f_flux(T0, dt) # compound the mean T = Gamma.mm(Phi).mm(Upsilon) # Jacobian for propagating prior along time F = torch.eye(9) F[6:9, 3:6] = torch.eye(3) * dt # compute Adjoint of right transformation mean AdUps = SE3_2.uAd(SE3_2.uinv(Upsilon)) Sigma_tmp = axat(AdUps.mm(F), Sigma) # compound the covariances based on the second-order method Sigma_prop = Sigma_tmp + Q if method == 3: # baseline SO(3) x R^6 wedge_acc = SO3.uwedge(Upsilon[:3, 3]) # already multiplied by dt F = torch.eye(9) F[3:6, :3] = T0[:3, :3].t() F[3:6, :3] = -T0[:3, :3].mm(wedge_acc) F[6:9, :3] = F[3:6, :3] * dt / 2 F[6:9, 3:6] = dt * torch.eye(3) G = torch.zeros(9, 6) G[:3, :3] = T0[:3, :3].t() G[3:6, 3:6] = T0[:3, :3] G[6:9, 3:6] = 1 / 2 * T0[:3, :3] * dt Sigma_prop = axat(F, Sigma) + axat(G, Q[:6, :6]) elif method == 4: # Monte Carlo method n_tot_samples = 100000 nsamples = 50000 N = int(n_tot_samples / nsamples) + 1 tmp = torch.cholesky(Sigma_prop + 1e-16 * torch.eye(9)) cholP = tmp.cuda().expand(nsamples, 9, 9) cholQ = cholQ.cuda().expand(nsamples, 9, 9) Sigma_prop = torch.zeros(9, 9) Gamma = Gamma.cuda().expand(nsamples, 5, 5) Upsilon = Upsilon.cuda().expand(nsamples, 5, 5) T0 = T0.cuda().expand(nsamples, 5, 5) T_inv = T.inverse().cuda().expand(nsamples, 5, 5) for i in range(N): xi0 = bmv(cholP, torch.randn(nsamples, 9).cuda()) w = bmv(cholQ, torch.randn(nsamples, 9).cuda()) T0_i = T0.bmm(SE3_2.exp(xi0)) Phi = f_flux(T0_i, dt) Upsilon_i = Upsilon.bmm(SE3_2.exp(w)) T_i = Gamma.bmm(Phi).bmm(Upsilon_i) xi = SE3_2.log(T_inv.bmm(T_i)) xi_mean = xi.mean(dim=0) Sigma_prop += bouter(xi - xi_mean, xi - xi_mean).sum(dim=0).cpu() Sigma_prop = Sigma_prop / (N * nsamples + 1) Sigma_prop = Sigma_prop / (N * nsamples + 1) Sigma_prop = (Sigma_prop + Sigma_prop.t()) / 2 return T, Sigma_prop
r = torch.randn(i_max, 9) r = r[r.norm(dim=1) < 4.1682][:, 6:9] P_est_chol = torch.cholesky(P_est[6:9, 6:9]) p_est = bmv(P_est_chol.expand(r.shape[0], 3, 3), r) + T_est[:3, 4] plt.scatter(p_est[:, i1], p_est[:, i2], color=color, s=2, alpha=0.5) if i1 == 0 and i2 == 1: np.savetxt(path2, p_est.numpy(), header="x y z", comments='') if __name__ == '__main__': i_max = 1000 # Propagate the uncertainty methods T_est = torch.eye(5) T_est[:3, 4] = torch.Tensor([5, 0, 0]) P_est = torch.diag(torch.Tensor([0.1, 0.1, 0.5, 1, 1, 1, 0.1, 0.1, 0.1])) P_est = axat(SE3_2.uAd(T_est.inverse()), P_est) SigmaSO3 = torch.eye(9) SigmaSO3[7, 7] = 5 SigmaSO3[6, 6] = 0.5 # saving paths path1 = "figures/retraction_b.txt" path2 = "figures/retraction_est.txt" labels = ['x (m)', 'y (m)', 'z (m)'] for i1, i2 in [(0, 1)]: plt.figure() # Plot the covariance of the samples T_est[:3, 4] = torch.Tensor([-4, 0, 0]) plot_so3_helper(T_est, SigmaSO3, "red", i1, i2, i_max, path1) plt.scatter(T_est[i1, 4], T_est[i2, 4], color='blue', s=20)