卡尔曼滤波学习笔记

卡尔曼滤波本质是（预测 + 更新）的两部递归算法，用于在有噪声的动态系统中估计系统状态。

核心思想

        系统模型

        状态方程：

                Xt​=Ft​ Xt−1​+Bt​ Ut​+Wt

​                        其中 wt∼N(0,Qk)是过程噪声（均值 0，协方差 Qt​）

                物理含义：

                        Xt：系统在t时刻的真实状态向量

                                例如：位置，速度，温度的内部变量。

                        Ft:状态转移矩阵，描述系统如何从K-1时刻的状态演化到K时刻。

                                例如：匀速直线运动时候，位置 pk=pk−1+v⋅Δtpk​，这可以写为矩阵形式。

                        Bt:控制输入矩阵，系统内所有的已知的外部控制变量，以及它如何影响系统状态

                                例如：加速指令，推力。

                        Wt：过程噪声，影响状态的外部干扰，模型的不确定性。

                 理解：如果没有噪声，描述系统内在动力学的模型。

        测量模型      

        观测方程：

                zt​=Ht​ Xt+vt

                        其中 vt∼N(0,Rt) 是观测噪声

                物理含义：

                        Zt：在K时刻测到的数据向量

                                例如：GPS读数，雷达距离，温度显示器

                        Ht:观测矩阵，描述状态XK如何映射到观测值。

                        Vt：观测噪声，传感器干扰，环境干扰等。

                        Xt：同一时刻的真实状态。

                理解： ​如果没有噪声，我们能从系统中读取到信息的模型

如何实现（预测+更新）

        预测

                先验状态预测

                        x^ t −​ =F⋅x^ t−1+ ​+B⋅ut​-1

                先验估计协方差

                        Pt−​=F⋅Pt−1​⋅FT+Q

                先验估计协方差推导

                 cov(X^t-,X^t-)=cov(FX^t-1+But-1+Wt,FX^t-1+But-1+Wt)

                                      =Fcov(X^t-1,X^t-1)FT+cov(wt,wt)

                  cov(wt,wt)=Q

                  cov(x^t-1,x^t-1)=Pt-1

        更新

                卡尔曼增益

                        Kt=(Pt−​⋅HT)/(H⋅Pt−​⋅HT+R)

                        当H,F为一维的时候，HT=H=F=FT=1 

                         =Pt-/(Pt-+R)=(Pt-1+Q)/(Pt-1+Q+R)

                         即卡尔曼增益与Q,R有关

                后验状态估计

                        x ^t​=x^t-+Kt⋅(zt​−H⋅x^t−​)

                                X^t是卡尔曼滤波的最终值 

                                x^t-是先验估计值,预测值

                                Zt是观测方程zt​=Ht​ Xt+vt中的观测值，也就是测量值

                                (zt-Hx^t-)就是误差，也就是（测量值-预测值）

                                 kt卡尔曼增益，也就是权重.

                                        当更信任预测值，可以把KT变小。

                                        当更信任观测值，可以把KT变大。

                后验协方差更新

                       Pt+​=(I−Kt⋅H)⋅Pt−​

调节超参数

Q,R

    K=(Pt-1+Q)/(Pt-1+Q+R)

                 当使用的是工业相机等观测值精度很高，很多高级的传感器。优先选择权重K比较大，再更具卡尔     

                 曼增益公式去选择对应的Q,R值。例如：K越大，权重越大，越信任观测值，Q越大，R越小。

                反之，如果你的系统模型很完美，没有摩擦的环境影响。选择估计值，K权重较小。

P0与X^0的取值

                习惯取x^0=0,P一般往小的取，方便收敛。（一般来说取1，不可以取0）

                后面迭代几次过后P0会收敛并且趋于一个稳定的值。

卡尔曼滤波的使用

1.选择状态量和观测量

2.构建方程

3.初始化参数P,X,Q,R,

4.代入预测公式和更新公式迭代循环

5.调教超参数