前言
卡尔曼滤波方程的推导有多种方式,有从随机过程的角度切入的,但这种方法涉及到很多积分及随机过程的知识,难以理解。也有从最小二乘的角度切入的,这种方式只需矩阵论及少量随机过程的知识,西工大的严恭敏老师在他的《捷联惯导算法与组合导航原理讲义》中详细介绍了这种方式,在B站也有相关的视频课程,这些也是笔者的主要参考源。
以前也推导过几遍卡尔曼滤波,但是过段时间又忘了,写这篇文章的目的在于总结并加深自己对卡尔曼滤波的印象及理解,同时方便自己梳理并回顾此部分知识,也希望对相关读者有所帮助,部分不详尽的地方可以去看看B站的视频课程。
最小二乘基础公式
递推最小二乘
第k次最小二乘量测模型:
将1,2,…,i次所有量测合并:
当i=k-1时,最优加权最小二乘估计为:
在i=k时,多了第k次量测,最优加权最小二乘估计为:
我们要利用上一时刻的状态、协方差以及当前时刻的量测来推出当前时刻的状态、协方差,也即我们需要构造一个如下的递推目标函数:
(1)方差阵递推
(2)状态估计递推
则递推最小二乘公式:
根据矩阵求逆引理
可得如下等式:
则递推最小二乘公式可改写为如下形式(相较于之前的形式,求逆的次数变少了):
卡尔曼滤波方程推导
^表示估计,~表示误差
随机系统状态空间模型
符号定义及推导
状态一步预测:
状态一步预测误差:
状态预测均方差阵:(不相关项为0)
量测一步预测:
量测一步预测误差:
量测预测均方差阵:
预测协方差:
滤波增益K的求解
根据递推最小二乘的公式我们可以合理假设卡尔曼滤波方程(“预测+修正”的形式):
则状态估计误差:
状态估计均方误差阵:
我们要达到最优估计,要使状态误差最小,就是要使Pk达到最小,即使Pk的迹tr(Pk)达到最小。因为tr(Pk)是关于K的二次型,只要对K求偏导然后找到极值点就能找到使tr(Pk)最小的K。
矩阵的迹相关求导公式:
tr(Pk)对K求偏导等于0可得:
将K回代入Pk可得:
至此,我们得到了卡尔曼滤波经典公式: