005 gtsam/examples/easyPoint2KalmanFilter.cpp

最新推荐文章于 2024-07-24 21:03:53 发布

xlsemmmmmm

最新推荐文章于 2024-07-24 21:03:53 发布

阅读量467

点赞数

分类专栏：因子图优化文章标签： c++

本文链接：https://blog.csdn.net/weixin_43848456/article/details/127580261

版权

因子图优化专栏收录该内容

27 篇文章 3 订阅

订阅专栏

easyPoint2KalmanFilter.cpp

一、数据类型
二、主函数

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

一、数据类型

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

二、主函数

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;
}

2.1 创建初始化的点

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

2.2 初始化一个key

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

2.3 创建一个ekf类型

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

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

2.4 预测

目的预测
$\mathop{\arg\max}\limits_{x_{1}}\space P(x1) = P(x1|x0) P(x0)$
在卡尔曼滤波中符号表示为
$x_{t+1|t}\space and\space P_{t+1|t}$
运动方程
$f(x_{t}) = x_{t+1|t}$
将其线性展开
$f(x_{t}) = F*x_{t} + B*u_{t} + w$
$F$ ->状态转移矩阵
$B$ ->控制输入模型
$w$ 是零均值，协方差矩阵 $Q$ 的高斯分布
在一些模型中 $Q=G*w*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");

2.5 更新/校正

接收到观测后进行更新
$P(x1|z1)\sim P(z1|x1)*P(x1)$
在卡尔曼滤波中需要一个测量模型
$h(x_{t}) = \hat{z}_{t}$
线性展开： $h(x_{t}) = H*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");

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");