目录
- 前言
- 最小二乘基础公式
- 递推最小二乘
- 卡尔曼滤波方程推导
- 符号定义及推导
- 滤波增益K的求解
前言
卡尔曼滤波方程的推导有多种方式，有从随机过程的角度切入的，但这种方法涉及到很多积分及随机过程的知识，难以理解。也有从最小二乘的角度切入的，这种方式只需矩阵论及少量随机过程的知识，西工大的严恭敏老师在他的《捷联惯导算法与组合导航原理讲义》中详细介绍了这种方式，在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可得：
至此，我们得到了卡尔曼滤波经典公式：
