本文深入浅出地介绍了卡尔曼滤波的基本概念和五个核心公式，包括运动方程、测量方程、误差处理以及权重计算。通过数学推导展示了如何利用卡尔曼滤波优化估计值，同时提供了简单的跟踪图示例。
一看就懂的卡尔曼滤波五个公式
任何物体的运动都有一个运动方程：
x k ^ = A x k − 1 + u + w , w ∈ ( 0 , Q ) (1) \hat{x_k} =Ax_{k-1}+u+w ,w\in (0,Q) \tag{1} xk^=Axk−1+u+w,w∈(0,Q)(1)式中，k为某一个时刻，u为输入量，x为k-1时刻系统真实状态,w为方差Q的高斯误差， x k ^ \hat{x_k} xk^为k时刻估计状态。
至此，根据这个系统状态我们有一个测量方程：
z k ^ = H x k ^ (2) \hat{z_k} =H\hat{x_k}\tag{2} zk^=Hxk^(2)式（2）被人称为先验。
这两个方程由于实际上 很多情况达不到标准导致了会有误差，然后方程变为：
z k = H x k ^ + v , v ∈ ( 0 , R ) (3) \\z_k=H\hat{x_k}+v,v\in (0,R)\tag{3} zk=Hxk^+v,v∈(0,R)(3)式中，v为方差R的高斯误差。
一般来说，式（3）中的 z k z_k zk是通过传感器测量出来的（测量值），式（2）中的 z k ^ \hat{z_k} zk^是我们通过运动方程转化过来的（理论值）。针对这两个值我们到底应该相信谁呢？当然是谁的表现好谁的权重大，从而可以建立一个方程：
H X n e w = ( 1 − K ) H x k ^ + K z k = H x k ^ + K ( z k − H x k ^ ) (5) HX_{new}=(1-K)H\hat{x_k}+Kz_k=H\hat{x_k}+K(z_k-H\hat{x_k})\tag{5} HXnew=(1−K)Hxk^
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
