一、背景知识
卡尔曼滤波器和其衍生出来的一系列滤波器被验证是行之有效的多源数据融合方法,在工程上有广泛的应用。应用的场景主要分两种:1. 预测无法直接测量的状态量;2.融合多源的信息得出更加准确的状态量。大家都知道卡尔曼滤波器分为预测和更新两个过程,一共有5个公式,但是很多人不知道公式是怎么推导出来的。这两篇文章会用两种方式详细推导卡尔曼滤波器的公式。
卡尔曼滤波器的5个经典公式如下:
预测(时间更新过程)
x
^
k
−
=
A
k
x
^
k
−
1
+
B
k
u
k
+
w
k
\hat x_k^{-}=A_k\hat x_{k-1}+B_ku_{k}+w_k
x^k−=Akx^k−1+Bkuk+wk
P
^
k
−
=
A
k
P
k
−
1
A
k
T
+
Q
k
\hat P_k^{-}=A_kP_{k-1}A_k^{T}+Q_k
P^k−=AkPk−1AkT+Qk
更新(状态更新过程)
K
k
=
P
k
−
H
K
T
(
H
k
P
k
−
H
k
T
+
R
k
)
−
1
K_k=P_k^{-}H_K^{T}(H_kP_k^{-}H_k^{T}+R_k)^{-1}
Kk=Pk−HKT(HkPk−HkT+Rk)−1
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
k
x
^
k
−
)
\hat x_k=\hat x_k^{-}+K_k(z_k-H_k\hat x_k^{-})
x^k=x^k−+Kk(zk−Hkx^k−)
P
k
=
P
k
−
−
K
k
H
k
P
k
−
P_k=P_{k}^{-}-K_kH_kP_k^{-}
Pk=Pk−−KkHkPk−
此外,在测量状态时有:
x
k
=
z
k
+
v
k
x_k=z_k+v_k
xk=zk+vk
以下是各个变量的含义:
x
k
x_k
xk: 状态向量的真实值
x
^
k
−
\hat x_k^{-}
x^k−: 状态向量的先验估计值
x
^
k
−
1
\hat x_{k-1}
x^k−1: 上个时刻状态向量的后验估计值
x
^
k
\hat x_k
x^k: 当前时刻状态向量的后验估计值
A
k
A_k
Ak: 状态转移矩阵
B
k
B_k
Bk: 状态控制矩阵
u
k
u_{k}
uk: 状态控制向量
w
k
w_k
wk: 过程噪声,符合正态分布
w
k
∼
N
(
0
,
Q
k
)
w_k \sim N(0, Q_k)
wk∼N(0,Qk)
P
^
k
−
\hat P_k^{-}
P^k−: 预测协方差矩阵的先验估计值
P
k
−
1
P_{k-1}
Pk−1: 上个时刻预测协方差矩阵的后验估计值
P
k
P_k
Pk: 当前时刻预测协方差矩阵的后验估计值
Q
k
Q_k
Qk: 过程噪声协方差矩阵
z
k
z_k
zk: 测量向量
v
k
v_k
vk: 测量噪声,符合正态分布
v
k
∼
N
(
0
,
R
k
)
v_k \sim N(0, R_k)
vk∼N(0,Rk)
R
k
R_k
Rk: 测量噪声的协方差矩阵
H
k
H_k
Hk: 状态向量到测量向量的转换矩阵
二、卡尔曼滤波公式的推导
在预测和更新阶段分别能得到两个近似状态向量真实值的值,记为
x
^
p
r
e
d
i
c
t
=
H
k
x
^
k
−
\hat x^{predict}=H_k\hat x_k^{-}
x^predict=Hkx^k−和
x
^
m
e
a
s
u
r
e
=
z
k
\hat x^{measure}=z_k
x^measure=zk。基于假设,预测的值和测量的值都是符合正态分布的,即
x
^
p
r
e
d
i
c
t
∼
N
(
u
0
,
σ
0
2
)
(
u
0
=
H
k
x
^
k
−
,
σ
0
2
=
H
k
P
k
−
H
k
T
)
\hat x^{predict}\sim N(u_0, \sigma_0^2)(u_0=H_k\hat x_k^{-}, \sigma_0^2=H_kP_k^{-}H_k^{T})
x^predict∼N(u0,σ02)(u0=Hkx^k−,σ02=HkPk−HkT),
x
^
m
e
a
s
u
r
e
∼
N
(
u
1
,
σ
1
2
)
(
u
1
=
z
k
,
σ
1
2
=
R
k
)
\hat x^{measure} \sim N(u_1,\sigma_1^2)(u_1=z_k, \sigma_1^2=R_k)
x^measure∼N(u1,σ12)(u1=zk,σ12=Rk),而最终得到的状态向量估计值
x
^
\hat x
x^的概率分布为这两个正态分布相乘的结果(注*)。下面计算一下两个正态分布函数相乘能得到什么?
N
(
x
,
u
0
,
σ
0
2
)
N
(
x
,
u
1
,
σ
1
2
)
=
1
2
π
σ
0
e
−
(
x
−
u
0
)
2
2
σ
0
2
1
2
π
σ
1
e
−
(
x
−
u
1
)
2
2
σ
1
2
=
1
2
π
σ
0
σ
1
e
−
(
x
−
u
0
)
2
2
σ
0
2
−
(
x
−
u
1
)
2
2
σ
1
2
\begin{aligned} N(x, u_0, \sigma_0^2)N(x, u_1, \sigma_1^2)= & \frac{1}{\sqrt{2\pi}\sigma_0}e^{-\frac{(x-u_0)^{2}}{2\sigma_0^{2}}}\frac{1}{\sqrt{2\pi}\sigma_1}e^{-\frac{(x-u_1)^{2}}{2\sigma_1^{2}}} \\ = & \frac{1}{2\pi\sigma_0\sigma_1}e^{-\frac{(x-u_0)^{2}}{2\sigma_0^{2}}-\frac{(x-u_1)^{2}}{2\sigma_1^{2}}} \end{aligned}
N(x,u0,σ02)N(x,u1,σ12)==2πσ01e−2σ02(x−u0)22πσ11e−2σ12(x−u1)22πσ0σ11e−2σ02(x−u0)2−2σ12(x−u1)2
指数部分用
−
β
-\beta
−β表示,则有
β
=
(
x
−
u
0
)
2
2
σ
0
2
+
(
x
−
u
1
)
2
2
σ
1
2
=
(
x
−
u
0
)
2
σ
1
2
+
(
x
−
u
1
)
2
σ
0
2
2
σ
0
2
σ
1
2
=
σ
1
2
x
2
−
2
σ
1
2
u
0
x
+
σ
1
2
u
0
2
+
σ
0
2
x
2
−
2
σ
0
2
u
1
x
+
σ
0
2
u
1
2
2
σ
0
2
σ
1
2
=
(
σ
0
2
+
σ
1
2
)
x
2
−
2
(
σ
1
2
u
0
+
σ
0
2
u
1
)
x
+
σ
1
2
u
0
2
+
σ
0
2
u
1
2
2
σ
0
2
σ
1
2
=
x
2
−
2
(
σ
1
2
u
0
+
σ
0
2
u
1
)
(
σ
0
2
+
σ
1
2
)
x
+
σ
1
2
u
0
2
+
σ
0
2
u
1
2
(
σ
0
2
+
σ
1
2
)
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
=
(
x
−
(
σ
1
2
u
0
+
σ
0
2
u
1
)
(
σ
0
2
+
σ
1
2
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
+
σ
1
2
u
0
2
+
σ
0
2
u
1
2
(
σ
0
2
+
σ
1
2
)
−
(
(
σ
1
2
u
0
+
σ
0
2
u
1
)
(
σ
0
2
+
σ
1
2
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
\begin{aligned} \beta= & \frac{(x-u_0)^{2}}{2\sigma_0^{2}} +\frac{(x-u_1)^{2}}{2\sigma_1^{2}} \\ = & \frac{(x-u_0)^{2}\sigma_1^2+(x-u_1)^{2}\sigma_0^2}{2\sigma_0^2\sigma_1^2} \\ = & \frac{\sigma_1^2x^2-2\sigma_1^2u_0x+\sigma_1^2u_0^2+\sigma_0^2x^2-2\sigma_0^2u_1x+\sigma_0^2u_1^2}{2\sigma_0^2\sigma_1^2} \\ = & \frac{(\sigma_0^2+\sigma_1^2)x^2-2(\sigma_1^2u_0+\sigma_0^2u_1)x+\sigma_1^2u_0^2+\sigma_0^2u_1^2}{2\sigma_0^2\sigma_1^2} \\ = & \frac{x^2-\frac{2(\sigma_1^2u_0+\sigma_0^2u_1)}{(\sigma_0^2+\sigma_1^2)}x+\frac{\sigma_1^2u_0^2+\sigma_0^2u_1^2}{(\sigma_0^2+\sigma_1^2)}}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}} \\ = & \frac{(x-\frac{(\sigma_1^2u_0+\sigma_0^2u_1)}{(\sigma_0^2+\sigma_1^2)})^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}}+ \frac{\frac{\sigma_1^2u_0^2+\sigma_0^2u_1^2}{(\sigma_0^2+\sigma_1^2)}-(\frac{(\sigma_1^2u_0+\sigma_0^2u_1)}{(\sigma_0^2+\sigma_1^2)})^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}} \end{aligned}
β======2σ02(x−u0)2+2σ12(x−u1)22σ02σ12(x−u0)2σ12+(x−u1)2σ022σ02σ12σ12x2−2σ12u0x+σ12u02+σ02x2−2σ02u1x+σ02u122σ02σ12(σ02+σ12)x2−2(σ12u0+σ02u1)x+σ12u02+σ02u12(σ02+σ12)2σ02σ12x2−(σ02+σ12)2(σ12u0+σ02u1)x+(σ02+σ12)σ12u02+σ02u12(σ02+σ12)2σ02σ12(x−(σ02+σ12)(σ12u0+σ02u1))2+(σ02+σ12)2σ02σ12(σ02+σ12)σ12u02+σ02u12−((σ02+σ12)(σ12u0+σ02u1))2
令
γ
=
(
x
−
(
σ
1
2
u
0
+
σ
0
2
u
1
)
(
σ
0
2
+
σ
1
2
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
\gamma=\frac{(x-\frac{(\sigma_1^2u_0+\sigma_0^2u_1)}{(\sigma_0^2+\sigma_1^2)})^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}}
γ=(σ02+σ12)2σ02σ12(x−(σ02+σ12)(σ12u0+σ02u1))2,
λ
=
σ
1
2
u
0
2
+
σ
0
2
u
1
2
(
σ
0
2
+
σ
1
2
)
−
(
(
σ
1
2
u
0
+
σ
0
2
u
1
)
(
σ
0
2
+
σ
1
2
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
\lambda=\frac{\frac{\sigma_1^2u_0^2+\sigma_0^2u_1^2}{(\sigma_0^2+\sigma_1^2)}-(\frac{(\sigma_1^2u_0+\sigma_0^2u_1)}{(\sigma_0^2+\sigma_1^2)})^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}}
λ=(σ02+σ12)2σ02σ12(σ02+σ12)σ12u02+σ02u12−((σ02+σ12)(σ12u0+σ02u1))2,可以看出来
λ
\lambda
λ为一个常数项,而
γ
\gamma
γ符合正态分布,可以将
γ
\gamma
γ继续转化为:
γ
=
(
x
−
(
σ
1
2
u
0
+
σ
0
2
u
1
)
(
σ
0
2
+
σ
1
2
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
=
(
x
−
(
u
0
+
(
σ
0
2
u
1
−
σ
0
2
u
0
)
(
σ
0
2
+
σ
1
2
)
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
=
(
x
−
(
u
0
+
σ
0
2
(
u
1
−
u
0
)
(
σ
0
2
+
σ
1
2
)
)
)
2
2
σ
0
2
σ
1
2
(
σ
0
2
+
σ
1
2
)
=
(
x
−
(
u
0
+
G
(
u
1
−
u
0
)
)
)
2
2
(
σ
0
2
−
G
σ
0
2
)
其中
G
=
σ
0
2
σ
0
2
+
σ
1
2
\begin{aligned} \gamma= & \frac{(x-\frac{(\sigma_1^2u_0+\sigma_0^2u_1)}{(\sigma_0^2+\sigma_1^2)})^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}} \\ = & \frac{(x-(u_0+\frac{(\sigma_0^2u_1-\sigma_0^2u_0)}{(\sigma_0^2+\sigma_1^2)}))^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}} \\ = & \frac{(x-(u_0+\frac{\sigma_0^2(u_1-u_0)}{(\sigma_0^2+\sigma_1^2)}))^2}{\frac{2\sigma_0^2\sigma_1^2}{(\sigma_0^2+\sigma_1^2)}} \\ = & \frac{(x-(u_0+G(u_1-u_0)))^2}{2(\sigma_0^2-G\sigma_0^2)} & \text{其中$G=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}$} \end{aligned}
γ====(σ02+σ12)2σ02σ12(x−(σ02+σ12)(σ12u0+σ02u1))2(σ02+σ12)2σ02σ12(x−(u0+(σ02+σ12)(σ02u1−σ02u0)))2(σ02+σ12)2σ02σ12(x−(u0+(σ02+σ12)σ02(u1−u0)))22(σ02−Gσ02)(x−(u0+G(u1−u0)))2其中G=σ02+σ12σ02
令
u
′
=
u
0
+
G
(
u
1
−
u
0
)
u^{'}=u_0+G(u_1-u_0)
u′=u0+G(u1−u0),
σ
′
2
=
σ
0
2
−
G
σ
0
2
\sigma^{'2}=\sigma_0^2-G\sigma_0^2
σ′2=σ02−Gσ02,则
N
(
x
,
u
0
,
σ
0
2
)
N
(
x
,
u
1
,
σ
1
2
)
=
S
g
1
2
π
σ
′
e
−
(
x
−
u
′
)
2
2
σ
′
2
(
S
g
为一个常数)
\begin{aligned} N(x, u_0, \sigma_0^2)N(x, u_1, \sigma_1^2)= & S_g\frac{1}{\sqrt{2\pi}\sigma^{'}}e^{-\frac{(x-u^{'})^{2}}{2\sigma^{'2}}} & \text{($S_g$为一个常数)} \end{aligned}
N(x,u0,σ02)N(x,u1,σ12)=Sg2πσ′1e−2σ′2(x−u′)2(Sg为一个常数)
到这里,我们验证了两个正态分布函数相乘之后得到带缩放因子
S
g
=
1
2
π
(
σ
0
2
+
σ
1
2
)
e
−
(
u
0
−
u
1
)
2
2
(
σ
0
2
+
σ
1
2
)
S_g=\frac{1}{\sqrt{2\pi(\sigma_0^2+\sigma_1^2})}e^{\frac{-(u_0-u_1)^2}{2(\sigma_0^2+\sigma_1^2)}}
Sg=2π(σ02+σ12)1e2(σ02+σ12)−(u0−u1)2的正态分布函数,即有
x
^
∼
N
(
u
′
,
σ
′
2
)
\hat x\sim N(u^{'}, \sigma^{'2})
x^∼N(u′,σ′2)。接下来将各个变量代入公式有:
G
=
H
k
P
k
−
H
k
T
(
H
k
P
k
−
H
k
T
+
R
k
)
G=H_kP_k^{-}H_k^{T}(H_kP_k^{-}H_k^{T}+R_k)
G=HkPk−HkT(HkPk−HkT+Rk)
H
k
x
^
k
=
H
k
x
^
k
−
+
G
(
z
k
−
H
k
x
^
k
−
)
H_k\hat x_k=H_k\hat x_k^{-}+G(z_k-H_k\hat x_k^{-})
Hkx^k=Hkx^k−+G(zk−Hkx^k−)
H
k
P
k
H
k
T
=
H
k
P
k
−
H
k
T
−
G
H
k
P
k
−
H
k
T
H_kP_kH_k^{T}=H_kP_k^{-}H_k^{T}-GH_kP_k^{-}H_k^{T}
HkPkHkT=HkPk−HkT−GHkPk−HkT
令
K
k
=
H
k
−
1
G
K_k=H_k^{-1}G
Kk=Hk−1G,即为卡尔曼增益,则
K
k
=
P
k
−
H
k
T
(
H
k
P
k
−
H
k
T
+
R
k
)
K_k=P_k^{-}H_k^{T}(H_kP_k^{-}H_k^{T}+R_k)
Kk=Pk−HkT(HkPk−HkT+Rk)
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
k
x
^
k
−
)
\hat x_k=\hat x_k^{-}+K_k(z_k-H_k\hat x_k^{-})
x^k=x^k−+Kk(zk−Hkx^k−)
P
k
=
P
k
−
−
K
k
H
k
P
k
−
P_k=P_k^{-}-K_kH_kP_k^{-}
Pk=Pk−−KkHkPk−
以上完成了卡尔曼滤波器公式推导。
注*:这里的推导直接用了一个前提,即状态向量后验估计值 x ^ \hat x x^的概率分布为两个正态分布相乘的结果,该前提在后续文章再做详细探讨。下面我们可以先从曲线上直观理解。
绿色的曲线表示上一次的后验估计值的正态分布曲线,在经过状态转移矩阵
A
k
A_k
Ak之后,正态分布曲线变成了蓝色的曲线(前提是
A
k
A_k
Ak是线性的),红色的曲线则为测量值的正态分布曲线,我们的任务就是利用蓝色和红色两个正态分布曲线得到一个方差更小的正态分布曲线。
直接将蓝色和红色两个正态分布曲线相乘能够得到底下天蓝色填充的曲线,这条曲线也为正态分布曲线,这也验证了我们上面的计算结果没有问题。此外,可以发现相乘后的正态分布曲线的方差更小了,但是幅度经过了缩放,即对这个曲线进行积分的结果不为1,需要对其进行归一化。青色的曲线就是归一化后的正态分布曲线,也就是当前时刻后验估计值
x
^
k
\hat x_k
x^k的正态分布曲线。
以下为画图的代码:
import numpy as np
from matplotlib import pyplot as plt
x = np.arange(-10, 20, 0.01)
u1 = 3
sigma1 = 2
y1 = 1/(np.sqrt(2*np.pi)*sigma1)*np.exp(-(x-u1)*(x-u1)/(2*sigma1*sigma1))
u2 = 5
sigma2 = 2.5
y2 = 1/(np.sqrt(2*np.pi)*sigma2)*np.exp(-(x-u2)*(x-u2)/(2*sigma2*sigma2))
u3 = 8
sigma3 = 1.5
y3 = 1/(np.sqrt(2*np.pi)*sigma3)*np.exp(-(x-u3)*(x-u3)/(2*sigma3*sigma3))
y5 = y2 * y3
K = sigma2*sigma2/(sigma2*sigma2+sigma3*sigma3)
Sg = 1/np.sqrt(2*np.pi*(sigma2*sigma2+sigma3*sigma3))*np.exp(-(u2-u3)*(u2-u3)/(2*(sigma2*sigma2+sigma3*sigma3)))
u4 = u2 + K*(u3 - u2)
sigma4 = np.sqrt(sigma2*sigma2 - K*sigma2*sigma2)
y4 = 1/(np.sqrt(2*np.pi)*sigma4)*np.exp(-(x-u4)*(x-u4)/(2*sigma4*sigma4))
fig, axs = plt.subplots()
axs.plot(x, y1, 'g')
axs.text(u1, 1/(np.sqrt(2*np.pi)*sigma1), r"$\leftarrow N(\hat x_{k-1}, u_1, \sigma_1)$")
axs.plot(x, y2, 'b')
axs.text(u2, 1/(np.sqrt(2*np.pi)*sigma2), r"$\leftarrow N(\hat x_{k}^{-}, u_2, \sigma_2)$")
axs.plot(x, y3, 'r')
axs.text(u3, 1/(np.sqrt(2*np.pi)*sigma3), r"$\leftarrow N(z_{k}, u_3, \sigma_3)$")
axs.grid(linewidth = 0.5, linestyle = ':')
fig, axs1 = plt.subplots()
axs1.plot(x, y2, 'b')
axs1.text(u2, 1/(np.sqrt(2*np.pi)*sigma2), r"$\leftarrow N(\hat x_{k}^{-}, u_2, \sigma_2)$")
axs1.plot(x, y3, 'r')
axs1.text(u3, 1/(np.sqrt(2*np.pi)*sigma3), r"$\leftarrow N(z_{k}, u_3, \sigma_3)$")
axs1.plot(x, y5, 'y')
axs1.text(u4, Sg*1/(np.sqrt(2*np.pi)*sigma4), r"$\leftarrow N(\hat x_{k}^{-}, u_2, \sigma_2)N(z_{k}, u_3, \sigma_3)$")
axs1.fill_between(x, y5)
axs1.plot(x, y4, 'c')
axs1.text(u4, 1/(np.sqrt(2*np.pi)*sigma4), r"$\leftarrow N(\hat x_{k}, u_4, \sigma_4)$")
axs1.grid(linewidth = 0.5, linestyle = ':')
plt.show()
总结一下,在预测阶段和更新阶段我们能得到两个真实值的估计,它们都服从正态分布,直接对两个正态分布曲线相乘能够得到方差更小的正态分布曲线,使得最终估计的值更加接近真实值。