卡尔曼滤波器
卡尔曼滤波器是一个 ”optimal recursive data processing algorithm”(最优化自回归数据处理算法)。
先说一个例子:
假设我们要研究的对象是一个房间的温度。
1.根据经验,温度是恒定的,即上一分钟的温度等于现在这一分钟的温度,经验即预测,但这并不是完全可信的,即存在一定的误差。我们假设成高斯白噪声。
2.另外在房间里放一个温度计,实时检测房间的温度,这是观测值。同样地也存在误差,也是高斯白噪声。
我们现在的目标就是,用这两个值,结合它们各自的噪声,估算出房间的实际温度。假设要估算k时刻的实际温度值,首先需要知道k-1时刻的温度值。
1)预测值,假设k-1时刻房间温度为23度,则k时刻的温度应该也为23度(恒定)。同时该值的偏差为5(5是3的平方加上4的平方再开方,其中3为k-1时刻估算出的最优温度值的偏差3,4为对预测的偏差)。
2)测量值,假设为25度,同时偏差为4度。
那么实际值为多少?相信谁多一点,可用它们的协方差来判断:
,Kg = 0.78
则实际的温度值是:23+0.78*(25-23) = 24.56度。可以看出温度计测量的covariance较小,所以实际值比较偏向温度计。
在进入k+1时刻估算前,需要算出k时刻最优值(24.56)的偏差。算法,5即k时刻估算所用k-1时刻23度温度值的偏差。得出的2.35即k时刻最优值的偏差(对应上面k-1时刻的3)。
这样卡尔曼滤波器就不断得把covariance递归,从而估算出最优值。而且它只保留上一刻的covariance。上面的Kg就是卡尔曼增益(Kalman Gain)。
真正工程系统上的卡尔曼滤波器算法
首先引入一个离散控制过程的系统,改系统可用一个线性随机微分方程(linear stochastic difference equation)来描述:
再加上系统的测量值:
其中:X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。A和B是系统参数,对于多模系统,它们为矩阵。
Z(k)是k时刻的测量值,H是测量参数,对于多模系统,H为矩阵。W(k)和V(k)分别表示过程和测量的噪声,它们被假设成高斯白噪声,它们的covariance分别是Q,R。
对于满足上面条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。
卡尔曼滤波器算法流程及核心公式
首先利用系统过程模型,来预测下一个状态的系统ÿ