Ubuntu下的扩展卡尔曼滤波EKF程序（C++）

C++写的一个Ubuntu下扩展卡尔曼滤波程序，状态方程为线性，观测方程非线性，最后输出图片以便观察是否收敛，有比较详细的注释，分享给大家参考。还会上传一个matlab版本的。

#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;
using namespace cv;

int main（int argc char **argv） {
    
    int k=0;
    Point lasta lastb lastc;
    Eigen::Vector3d x_up（030M_PI/6）  x_pre（000）  xn（00M_PI/6） Rn（0.10.10.1）;  //沿x轴直线运动
    Eigen::Vector3d s_con（0.9*cos（M_PI/6） 0.9*sin（M_PI/6） 0）;
    Eigen::Matrix3d A=Eigen::Matrix3d::Identity（）;  //状态转移矩阵
    Eigen::Matrix3d R  P P_pre; //状态噪声方差观测噪声方差总协方差
    Eigen::Matrix2d Q S;
    Eigen::Matrix H; //观测方程雅克比矩阵
    Eigen::MatrixXd K; //卡尔曼增益
    Eigen::Vector2d z（01）;
    RNG gaus;  //高斯分布对象
    Mat img（500500CV_8UC3Scalar（255255255））;
    R<<0.10000.10000.1;
    Q<<0.1000.1;
    P<<0.10000.10000.1;
    //开始迭代
    for（int i=0; i<300; i++）
    {
     double gs=gaus.gaussian（1）;  //0均值，协方差为1的高斯分布中的随机数
      xn+=s_con+Rn*gs;   //真实值
      z<      
      //预测
      x_pre=x_