卡尔曼滤波算法详细推导（全网最详细的推导过程）
**
本文是来源于B站Dr_CAN的视频的学习笔记，有需要详细了解的，可以到B站看相关视频DR_CAN的个人空间
**
1、递归算法
例：
假设测一段距离，第一次测
z
1
z_1
z1=50.1mm，第二次测
z
2
z_2
z2=50.4mm，第三次测
z
3
z_3
z3=50.2mm，若想得到准确的值，可以计算这几次测量的平均值,
据此，可以定义估计真实数据等于平均值，可得到如下公式
x
^
k
=
1
k
(
x
1
+
x
2
+
⋯
x
k
)
\hat{x }_k=\frac{1}{k }(x_1+x_2+⋯x_k )
x^k=k1(x1+x2+⋯xk)
=
1
k
(
x
1
+
x
2
+
⋯
x
(
k
−
1
)
)
+
1
k
x
k
=\frac{1}{k }(x_1+x_2+⋯x_(k-1) )+\frac{1}{k } x_k
=k1(x1+x2+⋯x(k−1))+k1xk
=
(
k
−
1
)
k
(
k
−
1
)
(
x
1
+
x
2
+
⋯
x
(
k
−
1
)
)
+
1
k
x
k
=\frac{(k-1)}{k(k-1)} (x_1+x_2+⋯x_(k-1) )+\frac{1}{k } x_k
=k(k−1)(k−1)(x1+x2+⋯x(k−1))+k1xk
=
k
−
1
k
x
^
k
−
1
+
1
/
k
x
k
=\frac{k-1}{k} \hat{x }_{k-1}+1/k x_k
=kk−1x^k−1+1/kxk
=
x
^
(
k
−
1
)
−
1
/
k
x
^
(
k
−
1
)
+
1
k
x
k
=\hat{x }_(k-1)-1/k \hat{x }_(k-1)+\frac{1}{k } x_k
=x^(k−1)−1/kx^(k−1)+k1xk
于是有
x
^
k
=
x
^
k
−
1
+
1
k
(
x
k
−
x
^
k
−
1
)
\hat{x }_k=\hat{x }_{k-1}+\frac{1}{k } (x_k-\hat{x }_{k-1})
x^k=x^k−1+k1(xk−x^k−1)
据此可分析得到，随着k的数值增大，测量结果不再重要，可将上式描述为：
x
^
k
=
x
^
k
−
1
+
k
k
(
x
k
−
x
^
(
k
−
1
)
)
\hat{x}_k=\hat{x}_{k-1}+k_k (x_k-\hat{x}_{(k-1)})
x^k=x^k−1+kk(xk−x^(k−1))
即
当前的估计值=上一次的估计值+系数×（当前测量值-上一次估计值）
该式即为卡尔曼滤波的递归公式，可以对系数进行重新定义
为
k
k
=
E
e
k
−
1
E
e
k
−
1
+
M
e
k
k_k=\frac{Ee_{k-1}}{Ee_{k-1}+Me_k }
kk=Eek−1+MekEek−1
E
e
E_e
Ee是估计误差
M
e
M_e
Me是测量误差
2、数据融合、协方差矩阵、状态空间方程、观测器
（1）数据融合
例：假设两个传感器测得一物体的重量分别为
z
1
z_1
z1=30g，
z
2
z_2
z2=32g，其标准差分别为
σ
1
σ_1
σ1=2g,
σ
2
σ_2
σ2=4g，估计真实值
z
^
\hat z
z^
z
^
=
z
1
+
k
(
z
2
−
z
1
)
\hat z=z_1+k(z_2-z_1)
z^=z1+k(z2−z1)
求k，使
z
^
\hat z
z^的标准差
σ
z
^
σ_{\hat z }
σz^最小；
为了求使
σ
z
^
σ_{\hat z}
σz^最小的k值，可以对其进行求导：
即：
（2）协方差矩阵
略
3卡尔曼增益推导
已知状态空间：
实际状态空间模型：
于是有
旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。
更多推荐
22
0- 0
已为社区贡献1条内容
所有评论(0)