005 gtsam/examples/easyPoint2KalmanFilter.cpp

一个简单的2D point的卡尔曼滤波

一、数据类型

// Define Types for Linear System Test
typedef Point2 LinearMeasurement;
1
2

二、主函数

int main() {

  // Create the Kalman Filter initialization point
  Point2 x_initial(0.0, 0.0);
  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));

  // Create Key for initial pose
  Symbol x0('x',0);

  // Create an ExtendedKalmanFilter object
  ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);

  // Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0)
  // In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
  // For the Kalman Filter, this requires a motion model, f(x_{t}) = x_{t+1|t)
  // Assuming the system is linear, this will be of the form f(x_{t}) = F*x_{t} + B*u_{t} + w
  // where F is the state transition model/matrix, B is the control input model,
  // and w is zero-mean, Gaussian white noise with covariance Q
  // Note, in some models, Q is actually derived as G*w*G^T where w models uncertainty of some
  // physical property, such as velocity or acceleration, and G is derived from physics
  //
  // For the purposes of this example, let us assume we are using a constant-position model and
  // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
  // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
  Vector u = Vector2(1.0, 0.0);
  SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);

  // This simple motion can be modeled with a BetweenFactor
  // Create Key for next pose
  Symbol x1('x',1);
  // Predict delta based on controls
  Point2 difference(1,0);
  // Create Factor
  BetweenFactor<Point2> factor1(x0, x1, difference, Q);

  // Predict the new value with the EKF class
  Point2 x1_predict = ekf.predict(factor1);
  traits<Point2>::Print(x1_predict, "X1 Predict");



  // Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected"
  // This is equivalent to saying P(x1|z1) ~ P(z1|x1)*P(x1)
  // For the Kalman Filter, this requires a measurement model h(x_{t}) = \hat{z}_{t}
  // Assuming the system is linear, this will be of the form h(x_{t}) = H*x_{t} + v
  // where H is the observation model/matrix, and v is zero-mean, Gaussian white noise with covariance R
  //
  // For the purposes of this example, let us assume we have something like a GPS that returns
  // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
  // R = [0.25 0 ; 0 0.25].
  SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);

  // This simple measurement can be modeled with a PriorFactor
  Point2 z1(1.0, 0.0);
  PriorFactor<Point2> factor2(x1, z1, R);

  // Update the Kalman Filter with the measurement
  Point2 x1_update = ekf.update(factor2);
  traits<Point2>::Print(x1_update, "X1 Update");



  // Do the same thing two more times...
  // Predict
  Symbol x2('x',2);
  difference = Point2(1,0);
  BetweenFactor<Point2> factor3(x1, x2, difference, Q);
  Point2 x2_predict = ekf.predict(factor1);
  traits<Point2>::Print(x2_predict, "X2 Predict");

  // Update
  Point2 z2(2.0, 0.0);
  PriorFactor<Point2> factor4(x2, z2, R);
  Point2 x2_update = ekf.update(factor4);
  traits<Point2>::Print(x2_update, "X2 Update");



  // Do the same thing one more time...
  // Predict
  Symbol x3('x',3);
  difference = Point2(1,0);
  BetweenFactor<Point2> factor5(x2, x3, difference, Q);
  Point2 x3_predict = ekf.predict(factor5);
  traits<Point2>::Print(x3_predict, "X3 Predict");

  // Update
  Point2 z3(3.0, 0.0);
  PriorFactor<Point2> factor6(x3, z3, R);
  Point2 x3_update = ekf.update(factor6);
  traits<Point2>::Print(x3_update, "X3 Update");

  return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95

2.1 创建初始化的点

  Point2 x_initial(0.0, 0.0);
  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
1
2

2.2 初始化一个key

  // Create Key for initial pose
  Symbol x0('x',0);
1
2

2.3 创建一个ekf类型

  // Create an ExtendedKalmanFilter object
  ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);
1
2

构造函数参数列表：Key、初始值、噪声模型

2.4 预测

目的预测
$\underset{x_1}{\mathrm{argmax}}P(x1)=P(x1|x0)P(x0)$
在卡尔曼滤波中符号表示为
$x_{t+1|t}and{P}_{t+1|t}$
运动方程
$f(x_t)=x_{t+1|t}$
将其线性展开
$f(x_t)=F\ast x_t+B\ast u_t+w$
$F$->状态转移矩阵
$B$->控制输入模型
$w$是零均值，协方差矩阵$Q$的高斯分布
在一些模型中$Q=G\ast w\ast G^T$,$G$表示的不确定性是从实际模型导出的

  Vector u = Vector2(1.0, 0.0);
  SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);

  // This simple motion can be modeled with a BetweenFactor
  // Create Key for next pose
  Symbol x1('x',1);
  // Predict delta based on controls
  Point2 difference(1,0);
  // Create Factor
  BetweenFactor<Point2> factor1(x0, x1, difference, Q);

  // Predict the new value with the EKF class
  Point2 x1_predict = ekf.predict(factor1);
  traits<Point2>::Print(x1_predict, "X1 Predict");
1
2
3
4
5
6
7
8
9
10
11
12
13
14

2.5 更新/校正

接收到观测后进行更新
$P(x1|z1)\sim P(z1|x1)\ast P(x1)$
在卡尔曼滤波中需要一个测量模型
$h(x_t)={\hat{z}}_t$
线性展开：$h(x_t)=H\ast x_t+v$
$H$->观测模型
$v$是零均值、协方差是$R$的高斯白噪声模型

  SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);

  // This simple measurement can be modeled with a PriorFactor
  Point2 z1(1.0, 0.0);
  PriorFactor<Point2> factor2(x1, z1, R);

  // Update the Kalman Filter with the measurement
  Point2 x1_update = ekf.update(factor2);
  traits<Point2>::Print(x1_update, "X1 Update");
1
2
3
4
5
6
7
8
9

2.6 重复预测更新过程

  // Do the same thing two more times...
  // Predict
  Symbol x2('x',2);
  difference = Point2(1,0);
  BetweenFactor<Point2> factor3(x1, x2, difference, Q);
  Point2 x2_predict = ekf.predict(factor1);
  traits<Point2>::Print(x2_predict, "X2 Predict");

  // Update
  Point2 z2(2.0, 0.0);
  PriorFactor<Point2> factor4(x2, z2, R);
  Point2 x2_update = ekf.update(factor4);
  traits<Point2>::Print(x2_update, "X2 Update");



  // Do the same thing one more time...
  // Predict
  Symbol x3('x',3);
  difference = Point2(1,0);
  BetweenFactor<Point2> factor5(x2, x3, difference, Q);
  Point2 x3_predict = ekf.predict(factor5);
  traits<Point2>::Print(x3_predict, "X3 Predict");

  // Update
  Point2 z3(3.0, 0.0);
  PriorFactor<Point2> factor6(x3, z3, R);
  Point2 x3_update = ekf.update(factor6);
  traits<Point2>::Print(x3_update, "X3 Update");
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29