目录
前言
卡尔曼滤波方程的推导有多种方式，有从随机过程的角度切入的，但这种方法涉及到很多积分及随机过程的知识，难以理解。也有从最小二乘的角度切入的，这种方式只需矩阵论及少量随机过程的知识，西工大的严恭敏老师在他的《捷联惯导算法与组合导航原理讲义》中详细介绍了这种方式，在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可得：
至此，我们得到了卡尔曼滤波经典公式：
标签：推导,卡尔曼滤波,最小,Pk,递推,二乘 来源： https://blog.csdn.net/qq_41782151/article/details/117667419
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。