卡尔曼滤波算法详细推导

原创

已于 2025-01-23 17:10:52 修改 · 8.5w 阅读

·

168

·

CC 4.0 BY-SA版权

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

文章标签：

#滤波算法 #卡尔曼滤波

于 2018-09-27 16:44:39 首次发布

一、预备知识

1、协方差矩阵

是一个维列向量， u_i 是 x_i 的期望，协方差矩阵为

P=E[(X-E[X])(X-E[X])^T]

$=\begin{bmatrix} E[(x_1-u_1)(x_1-u_1)]& E[(x_1-u_1)(x_2-u_2)]& ...& E[(x_1-u_1)(x_n-u_n)]&\\ E[(x_2-u_2)(x_1-u_1)]& E[(x_2-u_2)(x_2-u_2)]& ...& E[(x_2-u_2)(x_n-u_n)]\\ ...& ...& ...& ...&\\ E[(x_n-u_n)(x_1-u_1)]& E[(x_n-u_n)(x_2-u_2)]& ...& E[(x_n-u_n)(x_n-u_n)]& \end{bmatrix}$

可以看出

协方差矩阵都是对称矩阵且是半正定的

协方差矩阵的迹 tr(P) 是的均方误差

2、用到的两个矩阵微分公式

公式一：

$\frac{\partial tr(AB)}{\partial A}=B^T$

公式二：若是对称矩阵，则下式成立

$\frac{\partial tr(ABA^T)}{\partial A}=2AB$

tr表示矩阵的迹，具体推导过程参考相关矩阵分析教程

二、系统模型与变量说明

1、系统离散型状态方程如下

由k-1时刻到k时刻，系统状态预测方程

$X_k = AX_{k-1}+Bu_k +w_k$

系统状态观测方程

Z_k=HX_k+v_k

2、变量说明如下

：状态转移矩阵

u_k ：系统输入向量

：输入增益矩阵

w_k ：均值为0，协方差矩阵为，且服从正态分布的过程噪声

：测量矩阵

最低0.47元/天解锁文章

200万优质内容无限畅学

评论 49

被折叠的条评论为什么被折叠?

到【灌水乐园】发言

查看更多评论

添加红包

成就一亿技术人!

hope_wisdom

发出的红包

实付元

使用余额支付

点击重新获取

扫码支付

钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。