2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, 3 Atlanta, Georgia 30332-0415 6 See LICENSE for the license information 8 KalmanFilter unit tests. 9 Author: Frank Dellaert & Duy Nguyen Ta (Python) 24 u = np.array([1.0, 0.0])
28 z1 = np.array([1.0, 0.0])
29 z2 = np.array([2.0, 0.0])
30 z3 = np.array([3.0, 0.0])
35 expected0 = np.array([0.0, 0.0])
36 P00 = 0.01 * np.eye(2)
38 expected1 = np.array([1.0, 0.0])
40 I11 = np.linalg.inv(P01) + np.linalg.inv(R)
42 expected2 = np.array([2.0, 0.0])
43 P12 = np.linalg.inv(I11) + Q
44 I22 = np.linalg.inv(P12) + np.linalg.inv(R)
46 expected3 = np.array([3.0, 0.0])
47 P23 = np.linalg.inv(I22) + Q
48 I33 = np.linalg.inv(P23) + np.linalg.inv(R)
54 x_initial = np.array([0.0, 0.0])
55 P_initial = 0.01 * np.eye(2)
58 state = KF.init(x_initial, P_initial)
59 self.assertTrue(np.allclose(expected0, state.mean()))
60 self.assertTrue(np.allclose(P00, state.covariance()))
63 state = KF.predict(state, F, B, u, modelQ)
64 self.assertTrue(np.allclose(expected1, state.mean()))
65 self.assertTrue(np.allclose(P01, state.covariance()))
66 state = KF.update(state, H, z1, modelR)
67 self.assertTrue(np.allclose(expected1, state.mean()))
68 self.assertTrue(np.allclose(I11, state.information()))
71 state = KF.predict(state, F, B, u, modelQ)
72 self.assertTrue(np.allclose(expected2, state.mean()))
73 state = KF.update(state, H, z2, modelR)
74 self.assertTrue(np.allclose(expected2, state.mean()))
77 state = KF.predict(state, F, B, u, modelQ)
78 self.assertTrue(np.allclose(expected3, state.mean()))
79 state = KF.update(state, H, z3, modelR)
80 self.assertTrue(np.allclose(expected3, state.mean()))
82 if __name__ ==
"__main__":
def test_KalmanFilter(self)
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)