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 align_traj(self):
"""yaw only and position alignment at initial time"""
self.gt['rpys'] = SO3.to_rpy(self.gt['Rots'].cuda()).cpu()
self.iekf['rpys'] = SO3.to_rpy(self.iekf['Rots'].cuda()).cpu()
self.gt['ps'] -= self.gt['ps'][0].clone()
self.iekf['ps'] -= self.iekf['ps'][0].clone()
rpys = self.gt['rpys'][:2] - self.iekf['rpys'][:2]
Rot = SO3.from_rpy(rpys[:, 0], rpys[:, 1], rpys[:, 2])
Rot = Rot[0].repeat(self.iekf['ps'].shape[0], 1, 1)
self.iekf['Rots'] = Rot.bmm(self.iekf['Rots'])
self.iekf['vs'] = bmv(Rot, self.iekf['vs'])
self.iekf['ps'] = bmv(Rot, self.iekf['ps'])
self.iekf['rpys'] = SO3.to_rpy(self.iekf['Rots'].cuda()).cpu()
def plot_body_velocity_err(self):
title = "Body velocity error as function of time " + self.end_title
vs = bmv(self.gt['Rots'], self.gt['vs'])
hat_vs = bmv(self.iekf['Rots'], self.iekf['vs'])
err = vs - hat_vs
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{v}_n^y$ (km/h)')
axs[2].set(xlabel='$t$ (min)', ylabel='$\mathbf{v}_n^z$ (km/h)')
for i in range(3):
axs[i].plot(self.ts, err[:, i], color="blue")
axs[i].set_xlim(self.ts[0], self.ts[-1])
fig.legend([r'IEKF'])
self.savefig(axs, fig, 'body_velocity_error')