卡尔曼滤波原理以及C++代码demo_卡尔曼增益c++-优快云博客

本文链接：https://blog.youkuaiyun.com/qhu1600417010/article/details/138368696

卡尔曼滤波原理以及C++代码demo

今天，要来说一下关于卡尔曼滤波的一些老生常谈的东西，什么是卡尔曼滤波，卡尔曼滤波的步骤和卡尔曼滤波最终形式的由来。
首先引用部分的两个链接已经把卡尔曼滤波前世今生讲解的非常详细了，在这里对资料进行汇总，方便若干年后的自己能够看到符合自己理解的讲解，不用重写总结。

要搞明白卡尔曼滤波需要弄明白这几个问题: 1) 卡尔曼增益的计算公式由来。2)每个时刻都包含观测，预测和估计三个值，其中t时刻的预测是借助t-1时刻的估计以及状态转移方程得到，t时刻的估计是加权t时刻的观测和t时刻的预测得到。 3) 预测使用的转移转移矩阵是理想的，如果系统包含过程造噪声，那么状态转移得到的预测结果是不够准确的，

引用参考
都是非常好的资料，首推第三个

https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
https://www.bilibili.com/video/BV1Rh41117MT/?spm_id_from=333.337.search-card.all.click&vd_source=fab964ee184dd54323edcad70fe7ff61
https://www.kalmanfilter.net/CN/kalman1d_cn.html
https://www.zhihu.com/question/41823401

状态转移方程
协方差转移矩阵
观测矩阵
状态更新方程
协方差更新方程
卡尔曼增益推导

在这里插入图片描述

代码部分
卡尔曼滤波预测和更新两部分，卡尔曼滤波主要实现预测过程个更新过程；因此在代码实现的时候需要完成四部分

变量定义
2）变量形状设置(由构造函数实现)
3）初始化，送入初始的状态量和协方差
4）预测过程
使用状态转化方程完成状态量的先验估计，然后根据初始的协方差更新先验估计的协方差；
5）更新过程
首先需要从系统中得到当前时刻的观测量(该值是直接从系统中得到的，不需要再代码计算，因为是使用卡尔曼增益来平衡真实观察量与先验估计值的比例）；这里需要观测矩阵，和观测噪声的协方差；

#define _MYKALMAN_H
#define _MYKALMAN_H
#include<iostream>
#include <Eigen\Dense>

class KalmanFilter
{
private:
    int stateSize; //state variable's dimenssion
    int measSize; //measurement variable's dimession
    int uSize; //control variables's dimenssion
    Eigen::VectorXd x;  //状态量
    Eigen::VectorXd z;  //观测量
    Eigen::MatrixXd A;  //状态转移矩阵
    Eigen::MatrixXd B;  //控制矩阵
    Eigen::VectorXd u;  //输入矩阵
    Eigen::MatrixXd P;  //先验估计协方差
    Eigen::MatrixXd H;  //观测矩阵
    Eigen::MatrixXd R;  //测量噪声协方差
    Eigen::MatrixXd Q;  //过程噪声协方差
public:
    KalmanFilter(int stateSize_, int measSize_, int uSize_);
    ~KalmanFilter() {}
    void init(Eigen::VectorXd& x_, Eigen::MatrixXd& P_, Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_);
    Eigen::VectorXd predict(Eigen::MatrixXd& A_);
    Eigen::VectorXd predict(Eigen::MatrixXd& A_, Eigen::MatrixXd& B_, Eigen::VectorXd& u_);
    void update(Eigen::MatrixXd& H_, Eigen::VectorXd z_meas);
};

KalmanFilter::KalmanFilter(int stateSize_ = 0, int measSize_ = 0, int uSize_ = 0) :stateSize(stateSize_), measSize(measSize_), uSize(uSize_)
// sateSize状态量个数
// uSize输入的维度
// 
{
    if (stateSize == 0 || measSize == 0)
    {
        std::cerr << "Error, State size and measurement size must bigger than 0\n";
    }

    x.resize(stateSize);
    x.setZero();

    A.resize(stateSize, stateSize);
    A.setIdentity();

    u.resize(uSize);
    u.transpose();
    u.setZero();

    B.resize(stateSize, uSize);
    B.setZero();

    P.resize(stateSize, stateSize);
    P.setIdentity();

    H.resize(measSize, stateSize);
    H.setZero();

    z.resize(measSize);
    z.setZero();

    Q.resize(stateSize, stateSize);
    Q.setZero();

    R.resize(measSize, measSize);
    R.setZero();
}

//初始化，卡尔曼滤波初始化只需要初始化初始状态，初始协方差矩阵，初始过程噪声协方差，初始化观测噪声协方差
void KalmanFilter::init(Eigen::VectorXd& x_, Eigen::MatrixXd& P_, Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_)
{
    x = x_;
    P = P_;
    R = R_;
    Q = Q_;
}

//有输入矩阵和控制矩阵的卡尔曼滤波预测过程
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_, Eigen::MatrixXd& B_, Eigen::VectorXd& u_)
{
    A = A_;
    B = B_;
    u = u_;
    x = A * x + B * u;//根据时刻t-1的状态由状态转换举着预测时刻t的先验估计值，
                      //因为当前是使用系统的状态转移矩阵来进行预测时刻t的预测值，并不清楚过程噪声如何，所以不适用过程噪声
    Eigen::MatrixXd A_T = A.transpose();
    P = A * P * A_T + Q;//时刻t先验估计值的的协方差,Q为过程噪声的协方差
    return x;
}

//没有输入矩阵和控制矩阵的卡尔曼滤波预测过程
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_)
{
    A = A_;
    x = A * x;
    Eigen::MatrixXd A_T = A.transpose();
    P = A * P * A_T + Q;
    //  cout << "P-=" << P<< endl;
    return x;
}

void KalmanFilter::update(Eigen::MatrixXd& H_, Eigen::VectorXd z_meas)
// H_   观测矩阵
//z_means 观测量，由系统真实观测输入
{
    H = H_;
    Eigen::MatrixXd temp1, temp2, Ht;
    Ht = H.transpose();
    temp1 = H * P * Ht + R;
    temp2 = temp1.inverse();//(H*P*H'+R)^(-1)
    Eigen::MatrixXd K = P * Ht * temp2;
    z = H * x;
    x = x + K * (z_meas - z);
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize, stateSize);//Identity()单位阵
    P = (I - K * H) * P;
    //  cout << "P=" << P << endl;
}