def plot_velocity_in_body_frame(self):
title = "Body velocity as function of time " + self.end_title
true = bmv(self.gt['Rots'].transpose(1, 2), self.gt['vs'])
mean = bmv(self.iekf['Rots'].transpose(1, 2), self.iekf['vs'])
# get 3 sigma uncertainty
P = torch.diag_embed(self.iekf['Ps'][:, :6],
offset=0,
dim1=-2,
dim2=-1)
J = P.new_zeros(P.shape[0], 3, 6)
J[:, :, :3] = SO3.wedge(mean)
J[:, :, 3:6] = self.iekf['Rots'].transpose(1, 2)
std = J.bmm(P).bmm(J.transpose(1, 2)).diagonal(dim1=1, dim2=2).sqrt()
fig, axs = plt.subplots(3, 1, sharex=True, figsize=self.figsize)
axs[0].set(ylabel='$(\mathbf{R}_n^T\mathbf{v}_n)^x$ (km/h)',
title=title)
axs[1].set(ylabel='$(\mathbf{R}_n^T\mathbf{v}_n)^y$ (km/h)')
axs[2].set(xlabel='$t$ (min)',
ylabel='$(\mathbf{R}_n^T\mathbf{v}_n)^z$ (km/h)')
for i in range(3):
axs[i].plot(self.ts, true[:, i], color="black")
axs[i].plot(self.ts, mean[:, i], color="green")
axs[i].plot(self.ts, (mean + std)[:, i], color='green', alpha=0.5)
axs[i].plot(self.ts, (mean - std)[:, i], color='green', alpha=0.5)
axs[i].set_xlim(self.ts[0], self.ts[-1])
fig.legend([r'ground truth', r'IEKF', r'$3\sigma$'], ncol=3)
self.savefig(axs, fig, 'body_velocity')
def dump(self, address, seq, zupts, covs):
# turn cov
J = torch.eye(9).repeat(self.Ps.shape[0], 1, 1)
J[:, 3:6, :3] = SO3.wedge(self.vs)
J[:, 6:9, :3] = SO3.wedge(self.ps)
#self.Ps = axat(J, self.Ps[:, :9, :9])
path = os.path.join(address, seq, 'iekf.p')
mondict = {
'Rots': self.Rots,
'vs': self.vs,
'ps': self.ps,
'b_omegas': self.b_omegas,
'b_accs': self.b_accs,
'rs': self.rs,
'Ps': self.Ps.diagonal(dim1=1, dim2=2),
'zupts': zupts,
'covs': covs,
}
for k, v in mondict.items():
mondict[k] = v.float().detach().cpu()
pdump(mondict, path)