Kalman Filter

introduction

本文对卡尔曼滤波的推导思路进行介绍。

construction

• 基本假设
  
  1. 线性系统
  2. 零均值高斯噪声

由于高斯噪声的引入，该系统可用线性随机差分方程描述，由上面的假设能够写出其状态方程和观测方程

xk|k−1yk=Axk−1|k−1+Buk−1+qk−1=Hxk|k+rk

其中q和r为高斯噪声，对应的协方差矩阵分别为Qk−1和Rk。由上述方差可以看出，系统的状态（随机变量）服从高斯分布，系统的观测状态yk为某一向量，某一时刻的预测状态xk|k−1可以由上一时刻的估计状态xk−1|k−1根据模型导出。

preparation

为了简单起见我们略去外部输入的影响，如果xk−1|k−1∼N(μk−1,Σk−1),qk−1∼N(0,Qk−1)，同时假设xk−1|k−1和qk−1不相关（即不存在线性关系——这是合理的），即

E[(xk−1|k−1−μk−1)qTk−1]=E[qk−1(xk−1|k−1−μk−1)T]=0

那么由多元正态分布的性质（线性变换的正态性、线性组合的正态性）可知，xk|k−1=Axk−1|k−1+qk−1也服从某一正态分布。

于是我们可以直接求其均值和方差

E[xk]E[(xk−μk)(xk−μk)T]=E[Axk−1|k−1+qk−1]=AE[xk−1|k−1]+E[qk]=Aμk−1=[(Axk−1|k−1+qk−1−Aμk−1)(xTk−1|k−1AT+qTk−1−μTk−1AT)]=[(A(xk−1|k−1−μk−1)+qk−1)((xTk−1|k−1−μTk−1)AT+qTk−1)]=AE[(xk−1|k−1−μk−1)(xk−1|k−1−μk−1)T]AT+AE[(xk−1|k−1−μk−1)qTk−1]+E[qk−1(xk−1|k−1−μk−1)T]AT+E[qk−1qTk−1]=AΣk−1AT+Qk−1

令Pk−1=AΣk−1AT+Qk−1则xk|k−1∼N(Aμk−1,Pk−1)，同理xk|k|yk∼N(H−1yk,H−1Rk(HT)−1)。

另外，两个一维高斯分布N(ν1,γ21)和N(ν2,γ22)相乘的结果也是一维高斯分布，即N(νfused,γ2fused)并且容易得到

νfused=ν1+γ21γ21+γ22(ν1−ν2),γ2fused=γ21−γ41γ21+γ22

推广到多维高斯分布，将γ21,γ22替换为Γ1,Γ2、γ2fused,替换为Γfused，则

νfused=ν1+Γ1(Γ1+Γ2)−1(ν1−ν2),Γfused=Γ1−Γ1(Γ1+Γ2)−1Γ1

derivation

为了处理方便，我们令p(x¯)=p(xk|k−1),p(x^)=p(xk|k|yk)，如果将x¯和x^看作是估计状态xk|k分布的抽样，由最大似然估计有θ=argθmaxp(x|θ)=argθmaxp(x¯)p(x^)，即这两个分布的乘积形成的新分布就是估计状态的分布。

借助准备工作，同时注意到x¯和x^都服从相同维数的高斯分布，可以令ν1=Aμk−1,ν2=H−1yk以及Γ1=Pk−1,Γ2=H−1Rk(HT)−1，那么θ={νfused,γfused}={μk,Σk}，其中

μkΣk=Aμk−1+Pk−1[Pk−1+H−1Rk(HT)−1]−1(H−1yk−Aμk−1)=Aμk−1+[Pk−1HT(HPk−1HT+Rk)−1]H(H−1yk−Aμk−1)=Aμk−1+K(yk−HAμk−1)=Pk−1+Pk−1[Pk−1+H−1Rk(HT)−1]−1Pk−1=Pk−1+[Pk−1HT(HPk−1HT+Rk)−1]HPk−1=Pk−1+KHPk−1

其中K=Pk−1HT(HPk−1HT+Rk)−1为卡尔曼增益，这样我们就得到了估计状态的分布

xk|k∼N(μk,Σk)

一般情况下，可以直接取μk作为估计值，因为此处的概率密度最大，当然也可以根据实际情况，利用相关理论进行置信区间的求解。

summary

总之，卡尔曼滤波的方程如下

Pk−1KμkΣk=AΣk−1AT+Qk−1=Pk−1HT(HPk−1HT+Rk)−1=Aμk−1+K(yk−HAμk−1)=Pk−1+KHPk−1

其中A,H,μk−1,Σk−1,Qk−1,Rk都是已知量，Pk−1,K为中间量。

其基本思想为：估计分布=观测分布*预测分布、估计值=加权平均（观测值，预测值），这只有在高斯分布的假设下才成立。预测分布可以通过模型导出，观测分布可以通过其它途径得到。

卡尔曼滤波器是以最小均方误差为原则的最佳线性滤波器，采用实时的迭代算法而不要求保留过去的状态信息。

reference

1. Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation