def init_bias(self, ts, us, zupts):
"""Init IMU bias and orientation with first measurements"""
if zupts[:self.N_init].sum() == self.N_init:
N_init = self.N_init
else:
N_init = torch.where(zupts[:self.N_init] == 0)[0][0].item()
cprint('Bias initialized with only {} samples'.format(N_init),
'yellow')
u = us[:N_init].mean(dim=0)
gravity = -u[3:6]
self.Rot = self.SO3.from_2vectors(gravity, self.g)
self.b_omega = u[:3]
self.b_acc = self.Rot.t().mv(self.g) - gravity
self.P = self.P0.clone()
# init covariance
H = u.new_zeros(6, 15)
H[:3, 9:12] = self.Id3
H[3:6, :3] = self.Rot.t().mm(self.Wg)
H[3:6, 12:15] = -self.Id3
R = self.Q[:6, :6]
for i in range(N_init):
S = axat(H, self.P) + R
Kt, _ = torch.solve(self.P.mm(H.t()).t(), S)
K = Kt.t()
I_KH = self.IdP - K.mm(H)
self.P = axat(I_KH, self.P.clone()) + axat(K, R)
self.P = (self.P + self.P.t()).clone()/2
def update(self, i, u, cov, zupt):
H = self.H
if zupt == 1:
z = 1
else:
z = 0
H[:3, 3:6] = self.Rot.t()
H[0, 3:6] *= z
self.r = torch.cat((- self.Rot.t().mv(self.v),
u[:3] - self.b_omega,
u[3:6] - self.b_acc + self.Rot.t().mv(self.g)))
self.r[0] *= z
z *= self.r[3:6].norm() < self.max_omega_norm
z *= self.r[3:6].abs().max() < self.max_omega
z *= self.r[6:9].norm() < self.max_acc_norm
z *= self.r[6:9].abs().max() < self.max_acc
self.r[3:9] *= z
H[3:6, 9:12] = z*self.Id3
H[6:9, 12:15] = z*self.Id3
H[6:9, :3] = -z*self.Rot.t().mm(self.Wg)
R = torch.diag(torch.cat((cov,
self.zupt_omega_cov,
self.zupt_acc_cov), 0))
S = axat(H, self.P) + R
Kt, _ = torch.solve(self.P.mm(H.t()).t(), S)
K = Kt.t()
self.xi = K.mv(self.r)
self.state_update(i)
self.covariance_update(i, K, H, R)
def propagate_cov(self, i, dt, u, zupt):
F = self.F
G = self.G
Rot = self.Rot
z = 1 - zupt
F[3:6, :3] = z*self.Wg
F[3:6, 12:15] = -z*Rot
F[6:9, 3:6] = z*self.Id3
G[3:6, 3:6] = z*Rot
v_skew_rot = self.SO3.wedge(self.v).mm(Rot)
p_skew_rot = self.SO3.wedge(self.ps[i-1]).mm(Rot)
tmp = z*torch.cat((Rot, v_skew_rot, p_skew_rot))
G[:9, :3] = tmp
F[:9, 9:12] = -tmp
Phi = self.IdP + F*dt + 1/2*F.mm(F)*(dt**2)
P = axat(Phi, self.P + axat(G*dt, self.Q))
self.P = (P + P.t())/2
def covariance_update(self, i, K, H, R):
I_KH = self.IdP - K.mm(H)
P = axat(I_KH, self.P) + axat(K, R)
self.P = (P + P.t())/2