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
12/**
13 * @file testParticleFactor.cpp
14 * @brief Unit tests for the Particle factor
15 * @author Frank Dellaert
16 * @date Dec 9, 2013
17 */
18
19#include <gtsam/linear/NoiseModel.h>
20
21namespace gtsam {
22
23/**
24 * A factor approximating a density on a variable as a set of weighted particles
25 */
26template<class X>
27class ParticleFactor {
28
29public:
30 typedef ParticleFactor This; ///< Typedef to this class
31 typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
32
33};
34
35/**
36 * A particle filter class, styled on functional KalmanFilter
37 * A state is created, which is functionally updates through [predict] and [update]
38 */
39template<class X>
40class ParticleFilter {
41
42public:
43
44 /**
45 * The Particle filter state is simply a ParticleFactor
46 */
47 typedef typename ParticleFactor<X>::shared_ptr State;
48
49 /**
50 * Create initial state, i.e., prior density at time k=0
51 * In Bayes Filter notation, these are x_{0|0} and P_{0|0}
52 * @param x0 estimate at time 0
53 * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
54 */
55 State init(const Vector& x0, const SharedDiagonal& P0) {
56 return std::make_shared<ParticleFactor<X> >();
57 }
58
59};
60// ParticleFilter
61
62}// namespace gtsam
63
64#include <gtsam/slam/BetweenFactor.h>
65#include <gtsam/linear/KalmanFilter.h>
66#include <gtsam/geometry/Pose2.h>
67#include <CppUnitLite/TestHarness.h>
68
69using namespace std;
70using namespace gtsam;
71
72//******************************************************************************
73
74TEST( particleFactor, constructor ) {
75// ParticleFactor<Pose2> pf;
76 //CHECK(assert_equal(expected, actual));
77}
78
79//******************************************************************************
80// Tests to do:
81// Take two variables pf-x-*-y, eliminate x, multiply and sample then marginalize
82TEST( particleFactor, eliminate) {
83// ParticleFactor<Pose2> fx;
84 BetweenFactor<Pose2> fxy;
85
86}
87
88//******************************************************************************
89
90/** Small 2D point class implemented as a Vector */
91struct State: Vector {
92 State(double x, double y) :
93 Vector((Vector(2) << x, y).finished()) {
94 }
95};
96
97//******************************************************************************
98TEST( ParticleFilter, constructor) {
99
100// Create a Kalman filter of dimension 2
101 ParticleFilter<Pose2> pf1;
102
103// Create inital mean/covariance
104 State x_initial(0.0, 0.0);
105 SharedDiagonal P1 = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1);
106
107// Get initial state by passing initial mean/covariance to the filter
108 ParticleFilter<Pose2>::State p1 = pf1.init(x0: x_initial, P0: P1);
109
110// // Assert it has the correct mean, covariance and information
111// EXPECT(assert_equal(x_initial, p1->mean()));
112// Matrix Sigma = (Mat(2, 2) << 0.01, 0.0, 0.0, 0.01);
113// EXPECT(assert_equal(Sigma, p1->covariance()));
114// EXPECT(assert_equal(inverse(Sigma), p1->information()));
115//
116// // Create one with a sharedGaussian
117// KalmanFilter::State p2 = pf1.init(x_initial, Sigma);
118// EXPECT(assert_equal(Sigma, p2->covariance()));
119//
120// // Now make sure both agree
121// EXPECT(assert_equal(p1->covariance(), p2->covariance()));
122}
123
124//******************************************************************************
125TEST( ParticleFilter, linear1 ) {
126
127 // Create the controls and measurement properties for our example
128 Matrix F = I_2x2;
129 Matrix B = I_2x2;
130 Vector u = Vector2(1.0, 0.0);
131 SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1);
132 Matrix Q = 0.01 * I_2x2;
133 Matrix H = I_2x2;
134 State z1(1.0, 0.0);
135 State z2(2.0, 0.0);
136 State z3(3.0, 0.0);
137 SharedDiagonal modelR = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1);
138 Matrix R = 0.01 * I_2x2;
139
140// Create the set of expected output TestValues
141 State expected0(0.0, 0.0);
142 Matrix P00 = 0.01 * I_2x2;
143
144 State expected1(1.0, 0.0);
145 Matrix P01 = P00 + Q;
146 Matrix I11 = P01.inverse() + R.inverse();
147
148 State expected2(2.0, 0.0);
149 Matrix P12 = I11.inverse() + Q;
150 Matrix I22 = P12.inverse() + R.inverse();
151
152 State expected3(3.0, 0.0);
153 Matrix P23 = I22.inverse() + Q;
154 Matrix I33 = P23.inverse() + R.inverse();
155
156// Create a Kalman filter of dimension 2
157 KalmanFilter kf(2);
158
159// Create the Kalman Filter initialization point
160 State x_initial(0.0, 0.0);
161 SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(dim: 2, sigma: 0.1);
162
163// Create initial KalmanFilter object
164 KalmanFilter::State p0 = kf.init(x0: x_initial, P0: P_initial);
165 EXPECT(assert_equal(expected0, p0->mean()));
166 EXPECT(assert_equal(P00, p0->covariance()));
167
168// Run iteration 1
169 KalmanFilter::State p1p = kf.predict(p: p0, F, B, u, modelQ);
170 EXPECT(assert_equal(expected1, p1p->mean()));
171 EXPECT(assert_equal(P01, p1p->covariance()));
172
173 KalmanFilter::State p1 = kf.update(p: p1p, H, z: z1, model: modelR);
174 EXPECT(assert_equal(expected1, p1->mean()));
175 EXPECT(assert_equal(I11, p1->information()));
176
177// Run iteration 2 (with full covariance)
178 KalmanFilter::State p2p = kf.predictQ(p: p1, F, B, u, Q);
179 EXPECT(assert_equal(expected2, p2p->mean()));
180
181 KalmanFilter::State p2 = kf.update(p: p2p, H, z: z2, model: modelR);
182 EXPECT(assert_equal(expected2, p2->mean()));
183
184// Run iteration 3
185 KalmanFilter::State p3p = kf.predict(p: p2, F, B, u, modelQ);
186 EXPECT(assert_equal(expected3, p3p->mean()));
187 LONGS_EQUAL(3, (long)KalmanFilter::step(p3p));
188
189 KalmanFilter::State p3 = kf.update(p: p3p, H, z: z3, model: modelR);
190 EXPECT(assert_equal(expected3, p3->mean()));
191 LONGS_EQUAL(3, (long)KalmanFilter::step(p3));
192}
193
194//******************************************************************************
195int main() {
196 TestResult tr;
197 return TestRegistry::runAllTests(result&: tr);
198}
199//******************************************************************************
200
201