卡尔曼滤波详解(paper加源码)

本文通过实例解析卡尔曼滤波原理,利用MATLAB源码演示其在跟踪算法中的应用,从状态空间模型到预测与校正步骤,适合初学者理解高斯噪声下的最优估计。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

卡尔曼滤波广泛应用于智能导航与跟踪算法中,本文根据参考paper详解卡尔曼滤波原理,并结合matlab源码更好地理解paper

前期概念:
信号可以如下表示:
在这里插入图片描述
y k y_k yk是观测信号, a k a_k ak是增益, x k x_k xk是信息, n k n_k nk是noise
目标是估计 x k x_k xk
估计值 x ^ k \hat x_k x^k和真实值 x k x_k xk之间的误差为
在这里插入图片描述
如果用平方误差,则
在这里插入图片描述
估计一段时间的误差则是求期望值

在这里插入图片描述
用最大似然推导,即是最大化估计x时y的概率:

在这里插入图片描述
假设加性噪声是高斯分布,则:

在这里插入图片描述
K k K_k Kk是归一化常数,则最大似然函数(期望值)为

在这里插入图片描述
取log
在这里插入图片描述
卡尔曼滤波推导:
state空间
在过程中的状态:
在这里插入图片描述
x k x_k xk是k时刻的状态向量, Φ \Phi Φ是k时刻状态到k+1时刻状态的状态转移矩阵,是不随时间变化的, w k w_k wk是方差已知的噪声。
观测值
在这里插入图片描述
z k z_k zk是k时刻 x k x_k xk的实际观测值,H是状态向量到measurement向量的连接矩阵,不随时间变化, v k v_k vk是measurement error,不随时间变化,也是方差已知的白噪声。

前面最大似然处的推导可知,用MSE估计最优filter的前提是error必须是高斯分布。两个噪声的协方差用以下变量表示

在这里插入图片描述

假设 x ^ k ′ \hat x_k^\prime x^k x ^ k \hat x_k x^k的前一个估计, K k K_k Kk是卡尔曼增益,那么
在这里插入图片描述
所以

在这里插入图片描述
k时刻的误差协方差矩阵为

在这里插入图片描述
状态映射为下式
在这里插入图片描述
所以误差
在这里插入图片描述
因此
在这里插入图片描述
上式中 e k e_k ek w k w_k wk是不相关的,因为噪声 w k w_k wk只是在k时刻和k+1时刻之间的噪声,而误差 e k e_k ek则是从开始积累到k时刻的误差,因此
在这里插入图片描述

代码中上面的 Φ \Phi Φ用矩阵A表示

最终卡尔曼滤波的两步,predict和correct分别做如下的工作

在这里插入图片描述
具体怎么用,结合matlab代码中的predict和correct来说明
matlab代码可参照示例
示例为小球的跟踪

跟踪首先要有目标的初始位置,这个位置可以是人为规定,也可以是检测算法检测出来。该示例中采用检测算法得到初始位置,correct函数中也是用每帧检测到的位置来更新卡尔曼滤波的参数。colorImage为读入的每帧图像
在这里插入图片描述
第一帧的时候配置卡尔曼滤波的初始化参数
在这里插入图片描述
进入configureKalmanFilter函数,可看到配置了如下参数
在这里插入图片描述

在这里插入图片描述
其中Qs为[25, 10, 10]
在这里插入图片描述
如果已经不是初始位置了,之后的步骤如下:
如果判断检测的位置准确,可以用检测的位置来更新卡尔曼滤波的参数

具体步骤如下:
(1) predict函数得到 x ^ k − \hat x_k^- x^k P k − P_k^- Pk
实际观测值为 z k z_k zk,即检测到的位置
(2) correct函数更新参数
在这里插入图片描述
对应的代码中可看到,当检测到目标时,用检测的位置更新参数(predict + correct),没有检测到目标时,就仅更新 x ^ k − \hat x_k^- x^k P k − P_k^- Pk(predict)
在这里插入图片描述

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

蓝羽飞鸟

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值