Kalman滤波是一种递归过程,主要有两个更新过程:时间更新和观测更新,其中时间更新主要包括状态预测和协方差预测,主要是对系统的预测,而观测更新主要包括计算卡尔曼增益、状态更新和协方差更新,因此整个递归过程主要包括五个方面的计算:1)状态预测;2)协方差预测;3)卡尔曼增益;4)状态更新;5)协方差更新;
用数学公式表示,如下:
状态预测:
(1)其中,X(k|k)是k时刻的系统状态,U(k)是k时刻对系统的控制量(如果没有控制量,可以为零),
是状态转移矩阵,
是系统参数;
主要含义:X(k-1|k-1)是上一状态的最优结果,X(k|k-1)是利用上一状态预测得到的结果,这一步主要更新系统结果。
协方差预测:
(2)
其中,P(k-1|k-1)是上一状态X(k-1|k-1)对应的协方差,P(k|k-1)是X(k|k-1)对应的协方差,Q是系统过程噪声(假设为高斯白噪声)的协方差矩阵;
主要含义:对系统结果对应的协方差进行更新;
计算卡尔曼增益:
其中,K为卡尔曼增益,H为观测矩阵,R为测量噪声(同样假设为高斯白噪声)对应的协方差矩阵;
主要含义:用于状态更新和协方差更新中,因为前两个预测我们得到了当前状态的预测结果,然后再得到当前状态的观测值,通过预测值和观测值,以及增益,就可以得到当前状态的最优状态估计,也就是状态更新。
状态更新:
其中,Z(k)是k时刻的观测值;
主要含义:利用当前状态的观测值和预测值,及增益,计算k时刻下的最优状态估计值X(k|k),对应着公式(1)中的X(k-1|k-1);
协方差更新:
其中,I为单位矩阵;
主要含义:前面已经更新了状态,为了保证递推下去,还要跟新状态对应的协方差矩阵,对应着公式(2)中的P(k-1|k-1);
最后,注意既然是一个递归过程,就需要给出参数的初始值,递归过程是否很好的收敛取决于初始值的选取,所以要根据实际情况合适选择。