testKalmanFilter.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/linear/KalmanFilter.h>
21 #include <gtsam/linear/NoiseModel.h>
22 #include <gtsam/base/Testable.h>
23 #include <CppUnitLite/TestHarness.h>
24 
25 using namespace std;
26 using namespace gtsam;
27 
28 /* ************************************************************************* */
29 
31 struct State: Vector {
32  State(double x, double y) :
33  Vector((Vector(2) << x, y).finished()) {
34  }
35 };
36 
37 /* ************************************************************************* */
38 TEST( KalmanFilter, constructor ) {
39 
40  // Create a Kalman filter of dimension 2
41  KalmanFilter kf1(2);
42 
43  // Create inital mean/covariance
44  State x_initial(0.0, 0.0);
45  SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2, 0.1);
46 
47  // Get initial state by passing initial mean/covariance to the p
48  KalmanFilter::State p1 = kf1.init(x_initial, P1);
49 
50  // Assert it has the correct mean, covariance and information
51  EXPECT(assert_equal(x_initial, p1->mean()));
52  Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished();
53  EXPECT(assert_equal(Sigma, p1->covariance()));
54  EXPECT(assert_equal(Sigma.inverse(), p1->information()));
55 
56  // Create one with a sharedGaussian
57  KalmanFilter::State p2 = kf1.init(x_initial, Sigma);
58  EXPECT(assert_equal(Sigma, p2->covariance()));
59 
60  // Now make sure both agree
61  EXPECT(assert_equal(p1->covariance(), p2->covariance()));
62 }
63 
64 /* ************************************************************************* */
65 TEST( KalmanFilter, linear1 ) {
66 
67  // Create the controls and measurement properties for our example
68  Matrix F = I_2x2;
69  Matrix B = I_2x2;
70  Vector u = Vector2(1.0, 0.0);
71  SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
72  Matrix Q = 0.01*I_2x2;
73  Matrix H = I_2x2;
74  State z1(1.0, 0.0);
75  State z2(2.0, 0.0);
76  State z3(3.0, 0.0);
77  SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1);
78  Matrix R = 0.01*I_2x2;
79 
80  // Create the set of expected output TestValues
81  State expected0(0.0, 0.0);
82  Matrix P00 = 0.01*I_2x2;
83 
84  State expected1(1.0, 0.0);
85  Matrix P01 = P00 + Q;
86  Matrix I11 = P01.inverse() + R.inverse();
87 
88  State expected2(2.0, 0.0);
89  Matrix P12 = I11.inverse() + Q;
90  Matrix I22 = P12.inverse() + R.inverse();
91 
92  State expected3(3.0, 0.0);
93  Matrix P23 = I22.inverse() + Q;
94  Matrix I33 = P23.inverse() + R.inverse();
95 
96  // Create a Kalman filter of dimension 2
97  KalmanFilter kf(2);
98 
99  // Create the Kalman Filter initialization point
100  State x_initial(0.0, 0.0);
101  SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 0.1);
102 
103  // Create initial KalmanFilter object
104  KalmanFilter::State p0 = kf.init(x_initial, P_initial);
105  EXPECT(assert_equal(expected0, p0->mean()));
106  EXPECT(assert_equal(P00, p0->covariance()));
107 
108  // Run iteration 1
109  KalmanFilter::State p1p = kf.predict(p0, F, B, u, modelQ);
110  EXPECT(assert_equal(expected1, p1p->mean()));
111  EXPECT(assert_equal(P01, p1p->covariance()));
112 
113  KalmanFilter::State p1 = kf.update(p1p, H, z1, modelR);
114  EXPECT(assert_equal(expected1, p1->mean()));
115  EXPECT(assert_equal(I11, p1->information()));
116 
117  // Run iteration 2 (with full covariance)
118  KalmanFilter::State p2p = kf.predictQ(p1, F, B, u, Q);
119  EXPECT(assert_equal(expected2, p2p->mean()));
120 
121  KalmanFilter::State p2 = kf.update(p2p, H, z2, modelR);
122  EXPECT(assert_equal(expected2, p2->mean()));
123 
124  // Run iteration 3
125  KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ);
126  EXPECT(assert_equal(expected3, p3p->mean()));
127  LONGS_EQUAL(3, (long)KalmanFilter::step(p3p));
128 
129  KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR);
130  EXPECT(assert_equal(expected3, p3->mean()));
131  LONGS_EQUAL(3, (long)KalmanFilter::step(p3));
132 }
133 
134 /* ************************************************************************* */
135 TEST( KalmanFilter, predict ) {
136 
137  // Create dynamics model
138  Matrix F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1).finished();
139  Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8).finished();
140  Vector u = Vector3(1.0, 0.0, 2.0);
141  Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished();
142  Matrix M = trans(R)*R;
143  Matrix Q = M.inverse();
144 
145  // Create a Kalman filter of dimension 2
146  KalmanFilter kf(2);
147 
148  // Create the Kalman Filter initialization point
149  State x_initial(0.0, 0.0);
150  SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 1);
151 
152  // Create initial KalmanFilter state
153  KalmanFilter::State p0 = kf.init(x_initial, P_initial);
154 
155  // Ensure predictQ and predict2 give same answer for non-trivial inputs
156  KalmanFilter::State pa = kf.predictQ(p0, F, B, u, Q);
157  // We have A1 = -F, A2 = I_, b = B*u, pre-multipled with R to match Q noise model
158  Matrix A1 = -R*F, A2 = R;
159  Vector b = R*B*u;
160  SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0);
161  KalmanFilter::State pb = kf.predict2(p0, A1, A2, b, nop);
162  EXPECT(assert_equal(pa->mean(), pb->mean()));
163  EXPECT(assert_equal(pa->covariance(), pb->covariance()));
164 }
165 
166 /* ************************************************************************* */
167 // Test both QR and Cholesky versions in case of a realistic (AHRS) dynamics update
168 TEST( KalmanFilter, QRvsCholesky ) {
169 
170  Vector mean = Vector::Ones(9);
171  Matrix covariance = 1e-6 * (Matrix(9, 9) <<
172  15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6,
173  -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1,
174  0.0, -0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0,
175  0.0, 0.0, 0.0, 23.4, 24.5, -0.6, 0.0, 0.0, 0.0,
176  0.0, 0.0, 0.0, 24.5, 87.9, 10.1, 0.0, 0.0, 0.0,
177  0.0, 0.0, 0.0, -0.6, 10.1, 61.1, 0.0, 0.0, 0.0,
178  0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, 0.0,
179  63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0,
180  -0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0).finished();
181 
182  // Create two Kalman filter of dimension 9, one using QR the other Cholesky
183  KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::CHOLESKY);
184 
185  // create corresponding initial states
186  KalmanFilter::State p0a = kfa.init(mean, covariance);
187  KalmanFilter::State p0b = kfb.init(mean, covariance);
188 
189  // Set up dynamics update
190  Matrix Psi_k = 1e-6 * (Matrix(9, 9) <<
191  1000000.0, 0.0, 0.0, -19200.0, 600.0, -0.0, 0.0, 0.0, 0.0,
192  0.0, 1000000.0, 0.0, 600.0, 19200.0, 200.0, 0.0, 0.0, 0.0,
193  0.0, 0.0, 1000000.0, -0.0, -200.0, 19200.0, 0.0, 0.0, 0.0,
194  0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0,
195  0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0,
196  0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0,
197  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0,
198  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0,
199  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished();
200  Matrix B = Matrix::Zero(9, 1);
201  Vector u = Z_1x1;
202  Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) <<
203  33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
204  3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
205  -0.0, -0.3, 88.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
206  0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0,
207  0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0,
208  0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0,
209  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0, 0.0,
210  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0,
211  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2).finished();
212 
213  // Do prediction step
214  KalmanFilter::State pa = kfa.predictQ(p0a, Psi_k, B, u, dt_Q_k);
215  KalmanFilter::State pb = kfb.predictQ(p0b, Psi_k, B, u, dt_Q_k);
216 
217  // Check that they yield the same mean and information matrix
218  EXPECT(assert_equal(pa->mean(), pb->mean()));
219  EXPECT(assert_equal(pa->information(), pb->information(), 1e-7));
220 
221  // and in addition attain the correct covariance
222  Vector expectedMean = (Vector(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.).finished();
223  EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7));
224  EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7));
225  Matrix expected = 1e-6 * (Matrix(9, 9) <<
226  48.8, -3.1, -0.0, -0.4, -0.4, 0.0, 0.0, 63.8, -0.6,
227  -3.1, 148.4, -0.3, 0.5, 1.7, 0.2, -63.8, 0.0, -0.1,
228  -0.0, -0.3, 188.0, -0.0, 0.2, 1.2, 0.0, 0.1, 0.0,
229  -0.4, 0.5, -0.0, 23.6, 24.5, -0.6, 0.0, 0.0, 0.0,
230  -0.4, 1.7, 0.2, 24.5, 88.1, 10.1, 0.0, 0.0, 0.0,
231  0.0, 0.2, 1.2, -0.6, 10.1, 61.3, 0.0, 0.0, 0.0,
232  0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, 0.0,
233  63.8, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0,
234  -0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2).finished();
235  EXPECT(assert_equal(expected, pa->covariance(), 1e-7));
236  EXPECT(assert_equal(expected, pb->covariance(), 1e-7));
237 
238  // prepare update
239  Matrix H = 1e-3 * (Matrix(3, 9) <<
240  0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0,
241  -9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0,
242  -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.).finished();
243  Vector z = Vector3(0.2599 , 1.3327 , 0.2007);
244  Vector sigmas = Vector3(0.3323 , 0.2470 , 0.1904);
245  SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas);
246 
247  // do update
248  KalmanFilter::State pa2 = kfa.update(pa, H, z, modelR);
249  KalmanFilter::State pb2 = kfb.update(pb, H, z, modelR);
250 
251  // Check that they yield the same mean and information matrix
252  EXPECT(assert_equal(pa2->mean(), pb2->mean()));
253  EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7));
254 
255  // and in addition attain the correct mean and covariance
256  Vector expectedMean2 = (Vector(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882).finished();
257  EXPECT(assert_equal(expectedMean2, pa2->mean(), 1e-4));// not happy with tolerance here !
258  EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss?
259  Matrix expected2 = 1e-6 * (Matrix(9, 9) <<
260  46.1, -2.6, -0.0, -0.4, -0.4, 0.0, 0.0, 63.9, -0.5,
261  -2.6, 132.8, -0.5, 0.4, 1.5, 0.2, -64.0, -0.0, -0.1,
262  -0.0, -0.5, 188.0, -0.0, 0.2, 1.2, -0.0, 0.1, 0.0,
263  -0.4, 0.4, -0.0, 23.6, 24.5, -0.6, -0.0, -0.0, -0.0,
264  -0.4, 1.5, 0.2, 24.5, 88.1, 10.1, -0.0, -0.0, -0.0,
265  0.0, 0.2, 1.2, -0.6, 10.1, 61.3, -0.0, 0.0, 0.0,
266  0.0, -64.0, -0.0, -0.0, -0.0, -0.0, 647.2, -0.0, 0.0,
267  63.9, -0.0, 0.1, -0.0, -0.0, 0.0, -0.0, 647.2, 0.1,
268  -0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8).finished();
269  EXPECT(assert_equal(expected2, pa2->covariance(), 1e-7));
270  EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
271 
272  // do the above update again, this time with a full Matrix Q
273  Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal();
274  KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ);
275  KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ);
276 
277  // Check that they yield the same mean and information matrix
278  EXPECT(assert_equal(pa3->mean(), pb3->mean()));
279  EXPECT(assert_equal(pa3->information(), pb3->information(), 1e-7));
280 
281  // and in addition attain the correct mean and covariance
282  EXPECT(assert_equal(expectedMean2, pa3->mean(), 1e-4));
283  EXPECT(assert_equal(expectedMean2, pb3->mean(), 1e-4));
284 
285  EXPECT(assert_equal(expected2, pa3->covariance(), 1e-7));
286  EXPECT(assert_equal(expected2, pb3->covariance(), 1e-7));
287 }
288 
289 /* ************************************************************************* */
290 int main() {
291  TestResult tr;
292  return TestRegistry::runAllTests(tr);
293 }
294 /* ************************************************************************* */
295 
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
sampling::A1
static Matrix A1
Definition: testGaussianBayesNet.cpp:167
gtsam::utils.visual_isam.step
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
TEST
TEST(KalmanFilter, constructor)
Definition: testKalmanFilter.cpp:38
Q
Quaternion Q
Definition: testQuaternion.cpp:27
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
b
Scalar * b
Definition: benchVecAdd.cpp:17
simple::p3
static Point3 p3
Definition: testInitializePose3.cpp:53
Testable.h
Concept check for values that can be used in unit tests.
z2
static const Unit3 z2
Definition: testRotateFactor.cpp:43
gtsam::KalmanFilter::predictQ
State predictQ(const State &p, const Matrix &F, const Matrix &B, const Vector &u, const Matrix &Q) const
Definition: KalmanFilter.cpp:100
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
Definition: KalmanFilter.h:56
gtsam::KalmanFilter::predict
State predict(const State &p, const Matrix &F, const Matrix &B, const Vector &u, const SharedDiagonal &modelQ) const
Definition: KalmanFilter.cpp:89
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
R
Rot2 R(Rot2::fromAngle(0.1))
State::State
State(double x, double y)
Definition: testKalmanFilter.cpp:32
std
Definition: BFloat16.h:88
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
trans
static char trans
Definition: blas_interface.hh:58
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:12
sampling::mean
static const Vector2 mean(20, 40)
gtsam::KalmanFilter::predict2
State predict2(const State &p, const Matrix &A0, const Matrix &A1, const Vector &b, const SharedDiagonal &model) const
Definition: KalmanFilter.cpp:126
z3
static const Unit3 z3
Definition: testRotateFactor.cpp:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KalmanFilter::update
State update(const State &p, const Matrix &H, const Vector &z, const SharedDiagonal &model) const
Definition: KalmanFilter.cpp:135
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
main
int main()
Definition: testKalmanFilter.cpp:290
Eigen::QuaternionBase::inverse
EIGEN_DEVICE_FUNC Quaternion< Scalar > inverse() const
Definition: 3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h:720
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
gtsam
traits
Definition: chartTesting.h:28
KalmanFilter.h
Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF...
gtsam::KalmanFilter::init
State init(const Vector &x0, const SharedDiagonal &P0) const
Definition: KalmanFilter.cpp:65
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::KalmanFilter::updateQ
State updateQ(const State &p, const Matrix &H, const Vector &z, const Matrix &Q) const
Definition: KalmanFilter.cpp:145

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:33