def test_P(self):
self.assertEqual(self.msckf.P().shape, (21, 21))
# Plot matrix
# debug = True
debug = False
if debug:
ax = plt.subplot(111)
ax.matshow(self.msckf.P())
plt.show()