卡尔曼滤波

Posted 2021-01-27 yrm1160029237

在线性高斯系统中，状态方程、观测方程时线性的，且两个噪声项服从零均值的高斯分布。高斯分布经过线性变换后仍为高斯分布。

假设我们知道一个线性系统的状态差分方程为：

其中是k时刻系统的状态向量，n*1列向量；A系统转换矩阵 n*n， u为系统输入向量，大小k*1。B是将输入转换为状态的矩阵，大小n*k。 随机变量w为系统噪声，大小为n*1.

观测方程：

Z是测量值，大小为m*1(不是n*1，也不是1*1，后面将说明），H也是状态变量到测量的转换矩阵。大小为m*n。随机变量v是测量噪声。 测量噪声我们能够从传感器厂商那里获得噪声的均值

 

对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q,R为噪声变量的协方差矩阵。

我们认定是预测（先验）值，是估计值，为测量值的预测，在下面的推导中，请注意估计和预测两者的区别，不混为一谈。由一般的反馈思想我们得到估计值：

先看估计值与真实值间误差的协方差矩阵。

带入和得

展开得

预测值和真实值之间误差的协方差矩阵：

最后得到：

               

注意这个计算式K，转换矩阵H式常数，测量噪声协方差R也是常数。因此K的大小将与预测值的误差协方差有关。不妨进一步假设，上面式子中的矩阵维数都是1*1大小的，并假设H=1，不等于0。那么K可以写成如下：

                          

所以越大，那么K就越大，权重将更加重视反馈,如果等于0，也就是预测值和真实值相等，那么K=0，估计值就等于预测值（先验）。

将计算出的这个K反代入Pk中，就能简化Pk，估计协方差矩阵Pk的：

                                 

 

因此递推公式中每一步的K就计算出来了，同时每一步的估计协方差也能计算出来。但K的公式中好像又多了一个我们还未曾计算出来的东西,他称之为预测值和真实值之间误差的协方差矩阵。它的递推计算如下：

 请先注意到预测值的递推形式是：     

 

        

        

由于系统状态变量和噪声之间是独立，故可以写成：

      

                                             

由此也得到了的递推公式。因此我们只需设定最初的,就能不断递推下去。

这里总结下递推的过程，理一下思路：

首先要计算预测值、预测值和真实值之间误差协方差矩阵。

                    

有了这两个就能计算卡尔曼增益K，再然后得到估计值，

                     

最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。

                     

 

经验：

估计值误差协方差Pk的选取。

我们知道卡尔曼增益K与预测误差协方差矩阵正相关，由第一部分推导知道预测误差协方差阵：

                   

它又和估计误差协方差矩阵有关，在Q，A确定的情况下，和成正比。所以如果我们给的初值大的话，那么递推第一步中计算出的卡尔曼增益K就大。K大意味着更相信测量值。

当状态方程建立不正确的又会怎样呢？实际应用中很多时候我们不能建立正确的状态方程

你建立的越不正确，根据你模型进行的预测就不正确，从这个角度来说，相当于你的噪声增大了。所以这个时候系统噪声W的方差应该增大。理解这一点，对改进实际估计效果有好处

 

 举例：https://blog.csdn.net/young_gy/article/details/78177291