标签：partial 卡尔曼滤波 py 滤波 贝叶斯 frac hat px
- 0 前言
- 1 贝叶斯滤波的三大概率密度函数
- 2 扩展卡尔曼滤波的假设
- 3 扩展卡尔曼滤波的公式推导
- 4 矩阵形式的扩展卡尔曼滤波
- 5 应用实例——基于毫米波雷达与扩展卡尔曼滤波的目标跟踪
- 6 扩展卡尔曼滤波器的优缺点
- 7 参考
0 前言
扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。
EKF 的基本思想是利用泰勒级数展开将非线性系统的状态转移函数 f ( x ) f(x) f(x) 和（或）观测函数 h ( x ) h(x) h(x) 线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。
1 贝叶斯滤波的三大概率密度函数
在此前的文章《从概率到贝叶斯滤波》和《从贝叶斯滤波到卡尔曼滤波》中，已经讲到贝叶斯滤波的先验概率密度函数、似然概率密度函数和后验概率密度函数：
(1) 先验概率密度函数
f
X
k
−
(
x
)
=
∫
−
∞
+
∞
f
Q
k
[
x
−
f
(
v
)
]
f
X
k
−
1
+
(
v
)
d
v
f_{X_k}^-(x)=\int_{-\infty}^{+\infty}f_{Q_k}[x-f(v)]f_{X_{k-1}}^+(v)\mathrm{d}v
fXk−(x)=∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv
(2) 似然概率密度函数
f
Y
k
∣
X
k
(
y
k
∣
x
)
=
f
R
k
[
y
k
−
h
(
x
)
]
f_{Y_k|X_k}(y_k \ | \ x) = f_{R_k}[y_k-h(x)]
fYk∣Xk(yk ∣ x)=fRk[yk−h(x)]
(3) 后验概率密度函数
f
X
k
+
(
x
)
=
η
k
⋅
f
R
k
[
y
k
−
h
(
x
)
]
⋅
f
X
k
−
(
x
)
f_{X_k}^+(x)=\eta_k·f_{R_k}[y_k-h(x)]·f_{X_k}^-(x)
fXk+(x)=ηk⋅fRk[yk−h(x)]⋅fXk−(x)
其中，后验概率密度函数中的归一化常数
η
k
\eta_k
ηk 为：
η
k
=
{
∫
−
∞
+
∞
f
R
k
[
y
k
−
h
(
x
)
]
f
X
k
−
(
x
)
d
x
}
−
1
\eta_k=\left\{\int_{-\infty}^{+\infty}f_{R_k}[y_k-h(x)]f_{X_k}^-(x)\mathrm{d}x\right\}^{-1}
ηk={∫−∞+∞fRk[yk−h(x)]fXk−(x)dx}−1
2 扩展卡尔曼滤波的假设
扩展卡尔曼滤波以贝叶斯滤波为理论基础，并作了五个前提假设。
2.1 与卡尔曼滤波相同的假设
(1) 假设一：状态量服从正态分布
X
∼
N
(
μ
X
,
σ
X
2
)
X \sim \mathcal{N}(\mu_X, \ \sigma_X^2)
X∼N(μX, σX2)
(2) 假设二：观测量服从正态分布
Y
∼
N
(
μ
Y
,
σ
Y
2
)
Y \sim \mathcal{N}(\mu_Y, \ \sigma_Y^2)
Y∼N(μY, σY2)
(3) 假设三：过程噪声服从均值为 0 的正态分布
Q
∼
N
(
0
,
σ
Q
2
)
Q \sim \mathcal{N}(0, \ \sigma_Q^2)
Q∼N(0, σQ2)
(4) 假设四：观测噪声服从均值为 0 的正态分布
R
∼
N
(
0
,
σ
R
2
)
R \sim \mathcal{N}(0, \ \sigma_R^2)
R∼N(0, σR2)
2.2 与卡尔曼滤波不同的假设
(5) 假设五：状态转移函数和（或）观测函数为非线性函数
在卡尔曼滤波的前提假设中，认为状态方程中的状态转移函数 f ( x ) f(x) f(x) 以及观测方程中的观测函数 h ( x ) h(x) h(x) 均为线性函数。基于这种线性假设，存在常数或常矩阵 F F F，使得 f ( x ) f(x) f(x) 可以写成卡尔曼滤波中的线性形式，存在常数或常矩阵 H H H，使得 h ( x ) h(x) h(x) 也可以写成卡尔曼滤波中的线性形式。
不同于标准卡尔曼滤波，扩展卡尔曼滤波处理的是非线性系统，假设系统的状态转移函数和（或）观测函数为非线性函数。
3 扩展卡尔曼滤波的公式推导
3.1 预测步的两个公式
当状态转移函数为线性函数时，扩展卡尔曼滤波的预测步与标准卡尔曼滤波相同；当状态转移函数为非线性函数时，扩展卡尔曼滤波的预测步采用下面的推导过程。
根据假设一，
k
−
1
k-1
k−1 时刻状态量
X
k
−
1
X_{k-1}
Xk−1 服从均值为
μ
k
−
1
+
\mu_{k-1}^+
μk−1+，方差为
σ
k
−
1
+
2
{\sigma_{k-1}^+}^2
σk−1+2 的正态分布：
X
k
−
1
∼
N
(
μ
k
−
1
+
,
σ
k
−
1
+
2
)
X_{k-1} \sim \mathcal{N}(\mu_{k-1}^+, \ {\sigma_{k-1}^+}^2)
Xk−1∼N(μk−1+, σk−1+2)
X
k
−
1
X_{k-1}
Xk−1 的后验概率密度函数为：
f
X
k
−
1
+
=
1
σ
k
−
1
+
2
π
e
−
(
x
−
μ
k
−
1
+
)
2
2
σ
k
−
1
+
2
f_{X_{k-1}}^+=\frac{1}{\sigma_{k-1}^+\sqrt{2\pi}}e^{-\frac{(x-\mu_{k-1}^+)^2}{2{\sigma_{k-1}^+}^2}}
fXk−1+=σk−1+2π
1e−2σk−1+2(x−μk−1+)2
对状态转移函数
f
(
x
)
f(x)
f(x) 在
X
k
−
1
X_{k-1}
Xk−1 的后验估计处
x
^
k
−
1
+
\hat{x}_{k-1}^+
x^k−1+（即
μ
k
−
1
+
\mu_{k-1}^+
μk−1+）进行一阶泰勒级数展开：
f
(
X
k
−
1
)
=
f
(
x
^
k
−
1
+
)
+
f
′
(
x
^
k
−
1
+
)
(
X
k
−
1
−
x
^
k
−
1
+
)
+
o
(
X
k
−
1
−
x
^
k
−
1
+
)
f(X_{k-1})=f(\hat{x}_{k-1}^+)+f'(\hat{x}_{k-1}^+)(X_{k-1}-\hat{x}_{k-1}^+)+o(X_{k-1}-\hat{x}_{k-1}^+)
f(Xk−1)=f(x^k−1+)+f′(x^k−1+)(Xk−1−x^k−1+)+o(Xk−1−x^k−1+)
其中，
o
(
X
k
−
1
−
x
^
k
−
1
+
)
o(X_{k-1}-\hat{x}_{k-1}^+)
o(Xk−1−x^k−1+) 为高阶无穷小量，对其进行舍弃，便可得到状态转移函数
f
(
x
)
f(x)
f(x) 的近似形式：
f
(
X
k
−
1
)
≈
f
(
x
^
k
−
1
+
)
+
f
′
(
x
^
k
−
1
+
)
(
X
k
−
1
−
x
^
k
−
1
+
)
≈
f
′
(
x
^
k
−
1
+
)
(
X
k
−
1
)
+
[
f
(
x
^
k
−
1
+
)
−
f
′
(
x
^
k
−
1
+
)
x
^
k
−
1
+
]
\begin{aligned} f(X_{k-1}) & \approx f(\hat{x}_{k-1}^+)+f'(\hat{x}_{k-1}^+)(X_{k-1}-\hat{x}_{k-1}^+) \\ & \approx f'(\hat{x}_{k-1}^+)(X_{k-1})+[f(\hat{x}_{k-1}^+)-f'(\hat{x}_{k-1}^+)\hat{x}_{k-1}^+] \end{aligned}
f(Xk−1)≈f(x^k−1+)+f′(x^k−1+)(Xk−1−x^k−1+)≈f′(x^k−1+)(Xk−1)+[f(x^k−1+)−f′(x^k−1+)x^k−1+]
令
F
j
=
f
′
(
x
^
k
−
1
+
)
,
C
F
=
f
(
x
^
k
−
1
+
)
−
f
′
(
x
^
k
−
1
+
)
x
^
k
−
1
+
F_j=f'(\hat{x}_{k-1}^+), \quad C_F=f(\hat{x}_{k-1}^+)-f'(\hat{x}_{k-1}^+)\hat{x}_{k-1}^+
Fj=f′(x^k−1+),CF=f(x^k−1+)−f′(x^k−1+)x^k−1+，则近似有：
f
(
X
k
−
1
)
≈
F
j
∗
X
k
−
1
+
C
F
f(X_{k-1}) \approx F_j*X_{k-1}+C_F
f(Xk−1)≈Fj∗Xk−1+CF
结合假设三与上式，
k
k
k 时刻状态量
X
k
X_k
Xk 的先验概率密度函数为：
f
X
k
−
(
x
)
=
∫
−
∞
+
∞
f
Q
k
[
x
−
f
(
v
)
]
f
X
k
−
1
+
(
v
)
d
v
=
∫
−
∞
+
∞
1
σ
Q
k
2
π
e
−
(
x
−
F
j
∗
v
−
C
F
)
2
2
σ
Q
k
2
∗
1
σ
k
−
1
+
2
π
e
−
(
x
−
μ
k
−
1
+
)
2
2
σ
k
−
1
+
2
d
v
\begin{aligned} f_{X_k}^-(x) & = \int_{-\infty}^{+\infty}f_{Q_k}[x-f(v)]f_{X_{k-1}}^+(v)\mathrm{d}v \\ & = \int_{-\infty}^{+\infty}\frac{1}{\sigma_{Q_k}\sqrt{2\pi}}e^{-\frac{(x-F_j*v-C_F)^2}{2{\sigma_{Q_k}}^2}}*\frac{1}{\sigma_{k-1}^+\sqrt{2\pi}}e^{-\frac{(x-\mu_{k-1}^+)^2}{2{\sigma_{k-1}^+}^2}}\mathrm{d}v \end{aligned}
fXk−(x)=∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv=∫−∞+∞σQk2π
1e−2σQk2(x−Fj∗v−CF)2∗σk−1+2π
1e−2σk−1+2(x−μk−1+)2dv
类似推导标准卡尔曼滤波时的做法，使用 Mathematica 软件做符号推导，整理表达式结果可知，先验概率密度函数
f
X
k
−
(
x
)
f_{X_k}^-(x)
fXk−(x) 为正态分布函数，均值和方差分别为：
μ
k
−
=
F
j
∗
μ
k
−
1
+
+
C
F
=
f
(
μ
k
−
1
+
)
(3.1)
\mu_k^- = F_j*\mu_{k-1}^++C_F=f(\mu_{k-1}^+) \tag{3.1}
μk−=Fj∗μk−1++CF=f(μk−1+)(3.1)
σ k − 2 = F j 2 ∗ σ k − 1 + 2 + σ Q k 2 (3.2) {\sigma_k^-}^2 = F_j^2*{\sigma_{k-1}^+}^2+{\sigma_{Q_k}}^2 \tag{3.2} σk−2=Fj2∗σk−1+2+σQk2(3.2)
其中， F j = f ′ ( x ^ k − 1 + ) F_j=f'(\hat{x}_{k-1}^+) Fj=f′(x^k−1+)，对于一维扩展卡尔曼滤波， F j F_j Fj 为一常数项，即状态转移函数 f ( x ) f(x) f(x) 的一阶导函数 f ′ ( x ) f'(x) f′(x) 在上一时刻状态量后验估计处的函数值。
3.2 更新步的三个公式
当观测函数为线性函数时，扩展卡尔曼滤波的更新步与标准卡尔曼滤波相同；当观测函数为非线性函数时，扩展卡尔曼滤波的更新步采用下面的推导过程。
对观测函数
h
(
x
)
h(x)
h(x) 在
X
k
X_k
Xk 的先验估计处
x
^
k
−
\hat{x}_k^-
x^k−（即
μ
k
−
\mu_k^-
μk−）进行一阶泰勒级数展开：
h
(
X
k
)
=
h
(
x
^
k
−
)
+
h
′
(
x
^
k
−
)
(
X
k
−
x
^
k
−
)
+
o
(
X
k
−
x
^
k
−
)
h(X_k)=h(\hat{x}_k^-)+h'(\hat{x}_k^-)(X_k-\hat{x}_k^-)+o(X_k-\hat{x}_k^-)
h(Xk)=h(x^k−)+h′(x^k−)(Xk−x^k−)+o(Xk−x^k−)
其中，
o
(
X
k
−
x
^
k
−
)
o(X_k-\hat{x}_k^-)
o(Xk−x^k−) 为高阶无穷小量，对其进行舍弃，便可得到观测函数
f
(
x
)
f(x)
f(x) 的近似形式：
h
(
X
k
)
≈
h
(
x
^
k
−
)
+
h
′
(
x
^
k
−
)
(
X
k
−
x
^
k
−
)
≈
h
′
(
x
^
k
−
)
(
X
k
)
+
[
h
(
x
^
k
−
)
−
h
′
(
x
^
k
−
)
x
^
k
−
]
\begin{aligned} h(X_k) & \approx h(\hat{x}_k^-)+h'(\hat{x}_k^-)(X_k-\hat{x}_k^-) \\ & \approx h'(\hat{x}_k^-)(X_k)+[h(\hat{x}_k^-)-h'(\hat{x}_k^-)\hat{x}_k^-] \end{aligned}
h(Xk)≈h(x^k−)+h′(x^k−)(Xk−x^k−)≈h′(x^k−)(Xk)+[h(x^k−)−h′(x^k−)x^k−]
令
H
j
=
h
′
(
x
^
k
−
)
,
C
H
=
h
(
x
^
k
−
)
−
h
′
(
x
^
k
−
)
x
^
k
−
H_j=h'(\hat{x}_k^-), \quad C_H=h(\hat{x}_k^-)-h'(\hat{x}_k^-)\hat{x}_k^-
Hj=h′(x^k−),CH=h(x^k−)−h′(x^k−)x^k−，则近似有：
h
(
X
k
)
≈
H
j
∗
X
k
+
C
H
h(X_k) \approx H_j*X_k+C_H
h(Xk)≈Hj∗Xk+CH
结合假设四、公式 (3.1)、公式 (3.2) 及
h
(
x
)
h(x)
h(x) 的线性化近似形式，可知，k 时刻状态量
X
k
X_k
Xk 的后验概率密度函数为：
f
X
k
+
(
x
)
=
η
k
⋅
f
R
k
[
y
k
−
h
(
x
)
]
⋅
f
X
k
−
(
x
)
=
η
k
∗
1
σ
R
k
2
π
e
−
(
y
k
−
H
j
∗
x
−
C
H
)
2
2
σ
R
k
2
∗
1
σ
k
−
2
π
e
−
(
x
−
μ
k
−
)
2
2
σ
k
−
2
\begin{aligned} f_{X_k}^+(x) & = \eta_k·f_{R_k}[y_k-h(x)]·f_{X_k}^-(x) \\ & = \eta_k*\frac{1}{\sigma_{R_k}\sqrt{2\pi}}e^{-\frac{(y_k-H_j*x-C_H)^2}{2{\sigma_{R_k}}^2}}*\frac{1}{\sigma_{k}^-\sqrt{2\pi}}e^{-\frac{(x-\mu_{k}^-)^2}{2{\sigma_{k}^-}^2}} \end{aligned}
fXk+(x)=ηk⋅fRk[yk−h(x)]⋅fXk−(x)=ηk∗σRk2π
1e−2σRk2(yk−Hj∗x−CH)2∗σk−2π
1e−2σk−2(x−μk−)2
其中，归一化常数
η
k
\eta_k
ηk 为：
η
k
=
∫
−
∞
∞
1
σ
R
k
2
π
e
−
(
y
k
−
H
j
∗
x
−
C
H
)
2
2
σ
R
k
2
∗
1
σ
k
−
2
π
e
−
(
x
−
μ
k
−
)
2
2
σ
k
−
2
d
x
\eta_k=\int_{-\infty}^{\infty}\frac{1}{\sigma_{R_k}\sqrt{2\pi}}e^{-\frac{(y_k-H_j*x-C_H)^2}{2{\sigma_{R_k}}^2}}*\frac{1}{\sigma_{k}^-\sqrt{2\pi}}e^{-\frac{(x-\mu_{k}^-)^2}{2{\sigma_{k}^-}^2}}\mathrm{d}x
ηk=∫−∞∞σRk2π
1e−2σRk2(yk−Hj∗x−CH)2∗σk−2π
1e−2σk−2(x−μk−)2dx
类似推导标准卡尔曼滤波时的做法，使用 Mathematica 软件做符号推导，整理表达式结果可知，后验概率密度函数
f
X
k
+
(
x
)
f_{X_k}^+(x)
fXk+(x) 为正态分布函数，均值和方差分别为：
μ
k
+
=
μ
k
−
+
K
∗
(
y
k
−
H
j
∗
μ
k
−
−
C
H
)
=
μ
k
−
+
K
∗
[
y
k
−
h
(
μ
k
−
)
]
(3.3)
\mu_k^+ = \mu_k^-+K*(y_k-H_j*\mu_k^--C_H)= \mu_k^-+K*[y_k-h(\mu_k^-)] \tag{3.3}
μk+=μk−+K∗(yk−Hj∗μk−−CH)=μk−+K∗[yk−h(μk−)](3.3)
σ k + 2 = ( 1 − K ∗ H j ) ∗ σ k − 2 (3.4) {\sigma_k^+}^2 = (1-K*H_j)*{\sigma_k^-}^2 \tag{3.4} σk+2=(1−K∗Hj)∗σk−2(3.4)
μ
k
+
\mu_k^+
μk+ 即 k 时刻状态量
X
k
X_k
Xk 的后验估计
x
^
k
+
\hat{x}_k^+
x^k+。其中，
K
K
K 被称为卡尔曼增益系数：
K
=
H
j
∗
σ
k
−
2
H
j
2
∗
σ
k
−
2
+
σ
R
k
2
(3.5)
K=\frac{H_j*{\sigma_k^-}^2}{H_j^2*{\sigma_k^-}^2+{\sigma_{R_k}}^2} \tag{3.5}
K=Hj2∗σk−2+σRk2Hj∗σk−2(3.5)
其中， H j = h ′ ( x ^ k − ) H_j=h'(\hat{x}_k^-) Hj=h′(x^k−)，对于一维扩展卡尔曼滤波， H j H_j Hj 为一常数项，即观测函数 h ( x ) h(x) h(x) 的一阶导函数 h ′ ( x ) h'(x) h′(x) 在当前时刻状态量先验估计处的函数值。
4 矩阵形式的扩展卡尔曼滤波
上文内容所描述的是一维的扩展卡尔曼滤波，当状态量和观测量不再是单一的随机变量而是由多个随机变量组成的序列时，扩展卡尔曼滤波中各个量的维数也将随之改变：
- 状态量 X X X 由随机变量演变为随机向量，随机向量中的每一个分量为一个状态量随机变量。维数为 n X × 1 n_X \times 1 nX×1
- 状态转移比例项
F
j
F_j
Fj 演变为雅可比矩阵，维数为
n
X
×
n
X
n_X \times n_X
nX×nX。此时，
F
j
F_j
Fj 是由每个状态转移函数对每个状态量分量求偏导后，将上一时刻状态量的后验估计代入得到：
F j = ( ∂ f 1 ∂ X k − 1 1 ∂ f 1 ∂ X k − 1 2 ⋯ ∂ f 1 ∂ X k − 1 n X ∂ f 2 ∂ X k − 1 1 ∂ f 2 ∂ X k − 1 2 ⋯ ∂ f 2 ∂ X k − 1 n X ⋮ ⋮ ⋱ ⋮ ∂ f n X ∂ X k − 1 1 ∂ f n X ∂ X k − 1 2 ⋯ ∂ f n X ∂ X k − 1 n X ) ∣ X k − 1 = X ^ k − 1 + F_j= \begin{pmatrix} \frac{\partial f_1}{\partial X_{k-1}^1} & \frac{\partial f_1}{\partial X_{k-1}^2} & \cdots & \frac{\partial f_1}{\partial X_{k-1}^{n_X}} \\ \frac{\partial f_2}{\partial X_{k-1}^1} & \frac{\partial f_2}{\partial X_{k-1}^2} & \cdots & \frac{\partial f_2}{\partial X_{k-1}^{n_X}} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_{n_X}}{\partial X_{k-1}^1} & \frac{\partial f_{n_X}}{\partial X_{k-1}^2} & \cdots & \frac{\partial f_{n_X}}{\partial X_{k-1}^{n_X}} \end{pmatrix} \Bigg|_{X_{k-1}=\hat{X}_{k-1}^+} Fj=⎝⎜⎜⎜⎜⎜⎛∂Xk−11∂f1∂Xk−11∂f2⋮∂Xk−11∂fnX∂Xk−12∂f1∂Xk−12∂f2⋮∂Xk−12∂fnX⋯⋯⋱⋯∂Xk−1nX∂f1∂Xk−1nX∂f2⋮∂Xk−1nX∂fnX⎠⎟⎟⎟⎟⎟⎞∣∣∣∣∣Xk−1=X^k−1+ - 状态量概率密度函数均值 μ \mu μ 演变为矩阵，维数为 n X × 1 n_X \times 1 nX×1
- 状态量概率密度函数方差 σ 2 \sigma^2 σ2 演变为协方差矩阵，用 Σ \Sigma Σ 表示，维数为 n X × n X n_X \times n_X nX×nX
- 过程噪声方差 σ Q 2 {\sigma_Q}^2 σQ2 演变为协方差矩阵，用 Σ Q \Sigma_Q ΣQ 表示，维数为 n X × n X n_X \times n_X nX×nX
- 观测量 Y Y Y 由随机变量演变为随机向量，随机向量中的每一个分量为一个观测量随机变量。维数为 n Y × 1 n_Y \times 1 nY×1
- 观测值 y k y_k yk 由单一值演变为由单一值组成的值矩阵，维数为 n Y × 1 n_Y \times 1 nY×1
- 观测比例项
H
j
H_j
Hj 演变为雅可比矩阵，维数为
n
Y
×
n
X
n_Y \times n_X
nY×nX。此时，
H
j
H_j
Hj 是由每个观测函数对每个观测量分量求偏导后，将当前时刻状态量的先验估计代入得到：
H j = ( ∂ h 1 ∂ X k 1 ∂ h 1 ∂ X k 2 ⋯ ∂ h 1 ∂ X k n X ∂ h 2 ∂ X k 1 ∂ h 2 ∂ X k 2 ⋯ ∂ h 2 ∂ X k n X ⋮ ⋮ ⋱ ⋮ ∂ h n Y ∂ X k 1 ∂ h n Y ∂ X k 2 ⋯ ∂ h n Y ∂ X k n X ) ∣ X k = X ^ k − H_j= \begin{pmatrix} \frac{\partial h_1}{\partial X_k^1} & \frac{\partial h_1}{\partial X_k^2} & \cdots & \frac{\partial h_1}{\partial X_k^{n_X}} \\ \frac{\partial h_2}{\partial X_k^1} & \frac{\partial h_2}{\partial X_k^2} & \cdots & \frac{\partial h_2}{\partial X_k^{n_X}} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial h_{n_Y}}{\partial X_k^1} & \frac{\partial h_{n_Y}}{\partial X_k^2} & \cdots & \frac{\partial h_{n_Y}}{\partial X_k^{n_X}} \end{pmatrix} \Bigg|_{X_k=\hat{X}_k^-} Hj=⎝⎜⎜⎜⎜⎜⎛∂Xk1∂h1∂Xk1∂h2⋮∂Xk1∂hnY∂Xk2∂h1∂Xk2∂h2⋮∂Xk2∂hnY⋯⋯⋱⋯∂XknX∂h1∂XknX∂h2⋮∂XknX∂hnY⎠⎟⎟⎟⎟⎟⎞∣∣∣∣∣Xk=X^k− - 观测噪声方差 σ R 2 {\sigma_R}^2 σR2 演变为协方差矩阵，用 Σ R \Sigma_R ΣR 表示，维数为 n Y × n Y n_Y \times n_Y nY×nY
- 卡尔曼增益系数 K K K 演变为矩阵，维数为 n X × n Y n_X \times n_Y nX×nY
对应的五个公式演变为：
μ
k
−
=
f
(
μ
k
−
1
+
)
(4.1)
\mu_k^- = f(\mu_{k-1}^+) \tag{4.1}
μk−=f(μk−1+)(4.1)
Σ k − = F j ∗ Σ k − 1 + ∗ F j T + Σ Q k (4.2) \Sigma_k^- = F_j*\Sigma_{k-1}^+*F_j^T+{\Sigma_{Q_k}} \tag{4.2} Σk−=Fj∗Σk−1+∗FjT+ΣQk(4.2)
μ k + = μ k − + K ∗ [ y k − h ( μ k − ) ] (4.3) \mu_k^+ = \mu_k^-+K*[y_k-h(\mu_k^-)] \tag{4.3} μk+=μk−+K∗[yk−h(μk−)](4.3)
Σ k + = ( I − K ∗ H j ) ∗ Σ k − (4.4) \Sigma_k^+ = (I-K*H_j)*\Sigma_k^- \tag{4.4} Σk+=(I−K∗Hj)∗Σk−(4.4)
K = Σ k − ∗ H j T ∗ ( H j ∗ Σ k − ∗ H j T + Σ R k ) − 1 (4.5) K=\Sigma_k^-*H_j^T*(H_j*\Sigma_k^-*H_j^T+{\Sigma_{R_k}})^{-1} \tag{4.5} K=Σk−∗HjT∗(Hj∗Σk−∗HjT+ΣRk)−1(4.5)
公式 (4.3) 中 μ k + \mu_k^+ μk+ 即 k 时刻状态量 X k X_k Xk 的后验估计 x ^ k + \hat{x}_k^+ x^k+， y k − h ( μ k − ) y_k-h(\mu_k^-) yk−h(μk−) 常被称为残差（Residual）或新息（Innovation）；公式 (4.4) 中的 I I I 代表单位矩阵，维数为 n X × n X n_X \times n_X nX×nX。
当然，对于某个非线性系统，不一定状态转移和观测都是非线性的：
-
线性的状态转移 + 非线性的观测
此时，滤波递推公式由卡尔曼滤波的预测步两公式和扩展卡尔曼滤波的更新步三公式组成。 -
非线性的状态转移 + 线性的观测
此时，滤波递推公式由扩展卡尔曼滤波的预测步两公式和卡尔曼滤波的更新步三公式组成。
5 应用实例——基于毫米波雷达与扩展卡尔曼滤波的目标跟踪
5.1 系统分析
毫米波雷达（Radar）与激光雷达的的检测原理不同。激光雷达利用光的直线传播原理获得目标在笛卡尔坐标系下的距离信息；毫米波雷达利用电磁波的多普勒效应，获得目标在极坐标系下的距离 ρ \rho ρ、方向角 φ \varphi φ 和距离变化率（径向速度） ρ ˙ \dot{\rho} ρ˙。
假设我们使用毫米波雷达对某作匀速直线运动的目标在笛卡尔坐标系内的横坐标
p
x
p_x
px、纵坐标
p
y
p_y
py、横向速度
v
x
v_x
vx、纵向速度
v
y
v_y
vy 进行跟踪，则状态量可设为：
X
=
[
p
x
p
y
v
x
v
y
]
X= \begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \end{bmatrix}
X=⎣⎢⎢⎡pxpyvxvy⎦⎥⎥⎤
观测量可设为：
Y
=
[
ρ
φ
ρ
˙
]
Y= \begin{bmatrix} \rho \\ \varphi \\ \dot{\rho} \end{bmatrix}
Y=⎣⎡ρφρ˙⎦⎤
故，毫米波雷达的测量数据特性可用下图进行表征。
其中，距离变化率
ρ
˙
\dot{\rho}
ρ˙ 是绝对速度在径向上的投影。利用几何关系与三角函数进行推导，可知：
{
ρ
=
p
x
2
+
p
y
2
φ
=
arctan
(
p
y
p
x
)
ρ
˙
=
p
x
v
x
+
p
y
v
y
p
x
2
+
p
y
2
\begin{cases} \rho=\sqrt{ {p_x}^2+{p_y}^2} \\ \varphi=\text{arctan}(\frac{p_y}{p_x}) \\ \dot{\rho}=\frac{p_xv_x+p_yv_y}{\sqrt{ {p_x}^2+{p_y}^2} } \end{cases}
⎩⎪⎪⎨⎪⎪⎧ρ=px2+py2
φ=arctan(pxpy)ρ˙=px2+py2
pxvx+pyvy
故，观测函数
h
(
X
)
h(X)
h(X) 为：
h
(
X
)
=
(
h
1
h
2
h
3
)
=
(
p
x
2
+
p
y
2
arctan
(
p
y
p
x
)
p
x
v
x
+
p
y
v
y
p
x
2
+
p
y
2
)
h(X)= \begin{pmatrix} h_1 \\ h_2 \\ h_3 \end{pmatrix}= \begin{pmatrix} \sqrt{ {p_x}^2+{p_y}^2} \\ \text{arctan}(\frac{p_y}{p_x}) \\ \frac{p_xv_x+p_yv_y}{\sqrt{ {p_x}^2+{p_y}^2} } \end{pmatrix}
h(X)=⎝⎛h1h2h3⎠⎞=⎝⎜⎛px2+py2
arctan(pxpy)px2+py2
pxvx+pyvy⎠⎟⎞
h ( X ) h(X) h(X) 显然为非线性函数，而系统的状态转移（CV 模型）又明显是线性的，因此，此处的扩展卡尔曼滤波形式为：线性的状态转移 + 非线性的观测。
此时，滤波递推公式由卡尔曼滤波的预测步两公式和扩展卡尔曼滤波的更新步三公式组成：
卡尔曼滤波的预测步两公式
μ
k
−
=
F
∗
μ
k
−
1
+
+
B
∗
u
k
(5.1)
\mu_k^- = F*\mu_{k-1}^++B*u_k \tag{5.1}
μk−=F∗μk−1++B∗uk(5.1)
Σ k − = F ∗ Σ k − 1 + ∗ F T + Σ Q k (5.2) \Sigma_k^- = F*\Sigma_{k-1}^+*F^T+{\Sigma_{Q_k}} \tag{5.2} Σk−=F∗Σk−1+∗FT+ΣQk(5.2)
扩展卡尔曼滤波的更新步三公式
μ
k
+
=
μ
k
−
+
K
∗
[
y
k
−
h
(
μ
k
−
)
]
(5.3)
\mu_k^+ = \mu_k^-+K*[y_k-h(\mu_k^-)] \tag{5.3}
μk+=μk−+K∗[yk−h(μk−)](5.3)
Σ k + = ( I − K ∗ H j ) ∗ Σ k − (5.4) \Sigma_k^+ = (I-K*H_j)*\Sigma_k^- \tag{5.4} Σk+=(I−K∗Hj)∗Σk−(5.4)
K = Σ k − ∗ H j T ∗ ( H j ∗ Σ k − ∗ H j T + Σ R k ) − 1 (5.5) K=\Sigma_k^-*H_j^T*(H_j*\Sigma_k^-*H_j^T+{\Sigma_{R_k}})^{-1} \tag{5.5} K=Σk−∗HjT∗(Hj∗Σk−∗HjT+ΣRk)−1(5.5)
由于目标作匀速直线运动，故公式 (5.1) 中的控制项 B ∗ u k B*u_k B∗uk 可以忽略。
5.2 雅可比矩阵求解
公式 (5.4) 和公式 (5.5) 中的雅可比矩阵
H
j
H_j
Hj 根据定义可表示为：
H
j
=
[
∂
h
1
∂
p
x
∂
h
1
∂
p
y
∂
h
1
∂
v
x
∂
h
1
∂
v
y
∂
h
2
∂
p
x
∂
h
2
∂
p
y
∂
h
2
∂
v
x
∂
h
2
∂
v
y
∂
h
3
∂
p
x
∂
h
3
∂
p
y
∂
h
3
∂
v
x
∂
h
3
∂
v
y
]
H_j= \begin{bmatrix} \frac{\partial h_1}{\partial p_x} & \frac{\partial h_1}{\partial p_y} & \frac{\partial h_1}{\partial v_x} & \frac{\partial h_1}{\partial v_y} \\ \frac{\partial h_2}{\partial p_x} & \frac{\partial h_2}{\partial p_y} & \frac{\partial h_2}{\partial v_x} & \frac{\partial h_2}{\partial v_y} \\ \frac{\partial h_3}{\partial p_x} & \frac{\partial h_3}{\partial p_y} & \frac{\partial h_3}{\partial v_x} & \frac{\partial h_3}{\partial v_y} \end{bmatrix}
Hj=⎣⎢⎡∂px∂h1∂px∂h2∂px∂h3∂py∂h1∂py∂h2∂py∂h3∂vx∂h1∂vx∂h2∂vx∂h3∂vy∂h1∂vy∂h2∂vy∂h3⎦⎥⎤
直接求解
H
j
H_j
Hj 较为繁琐，可通过 matlab 的内置函数 jacobian() 进行求解。创建 matlab 脚本，命名为 CalculateHj.m 并输入如下内容后保存：
% define symbol variables
syms px py vx vy
% state random vector
X = [px py vx vy];
% measurement functions
h1 = sqrt(px^2 + py^2);
h2 = atan(py / px);
h3 = (px * vx + py * vy) / sqrt(px^2 + py^2);
% measurement jacobian matrix
Hj = jacobian([h1 h2 h3], X)
运行 CalculateHj.m 脚本，得到
H
j
H_j
Hj 如下运算结果：
Hj =
[ px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2), 0, 0]
[ -py/(px^2*(py^2/px^2 + 1)), 1/(px*(py^2/px^2 + 1)), 0, 0]
[ vx/(px^2 + py^2)^(1/2) - (px*(px*vx + py*vy))/(px^2 + py^2)^(3/2), vy/(px^2 + py^2)^(1/2) - (py*(px*vx + py*vy))/(px^2 + py^2)^(3/2), px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2)]
可以直接使用上述结果，也可以对其进行化简：
H
j
=
[
p
x
p
x
2
+
p
y
2
p
y
p
x
2
+
p
y
2
0
0
−
p
y
p
x
2
+
p
y
2
p
x
p
x
2
+
p
y
2
0
0
p
y
(
v
x
p
y
−
v
y
p
x
)
(
p
x
2
+
p
y
2
)
3
/
2
p
x
(
v
y
p
x
−
v
x
p
y
)
(
p
x
2
+
p
y
2
)
3
/
2
p
x
p
x
2
+
p
y
2
p
y
p
x
2
+
p
y
2
]
H_j= \begin{bmatrix} \frac{p_x}{\sqrt{ {p_x}^2+{p_y}^2} } & \frac{p_y}{\sqrt{ {p_x}^2+{p_y}^2} } & 0 & 0 \\ -\frac{p_y}{\sqrt{ {p_x}^2+{p_y}^2} } & \frac{p_x}{\sqrt{ {p_x}^2+{p_y}^2} } & 0 & 0 \\ \frac{p_y(v_xp_y-v_yp_x)}{({p_x}^2+{p_y}^2)^{3/2}} & \frac{p_x(v_yp_x-v_xp_y)}{({p_x}^2+{p_y}^2)^{3/2}} & \frac{p_x}{\sqrt{ {p_x}^2+{p_y}^2} } & \frac{p_y}{\sqrt{ {p_x}^2+{p_y}^2} } \end{bmatrix}
Hj=⎣⎢⎢⎡px2+py2
px−px2+py2
py(px2+py2)3/2py(vxpy−vypx)px2+py2
pypx2+py2
px(px2+py2)3/2px(vypx−vxpy)00px2+py2
px00px2+py2
py⎦⎥⎥⎤
5.3 代码
代码与此前的文章《（十三）手把手教你写卡尔曼滤波器》相似，区别在于状态量和观测量有所不同，且使用雅可比矩阵 H j H_j Hj 代替了观测矩阵 H H H（对于状态转移非线性的系统，还需使用雅可比矩阵 F j F_j Fj 代替状态转移矩阵 F F F）。
ExtendedKalmanFilter.h
#ifndef EXTENDED_KALMAN_FILTER_
#define EXTENDED_KALMAN_FILTER_
#include "D:/eigen3/Eigen/Dense"
class ExtendedKalmanFilter
{
private:
bool is_inited; // flag of initialization
bool just_begin_filt; // flag of just begining filt data
Eigen::VectorXd X; // state vector
Eigen::MatrixXd F; // state transition matrix
Eigen::MatrixXd P; // state covariance matrix
Eigen::MatrixXd Q; // process noise covariance matrix
Eigen::MatrixXd Hj; // measurement jacobian matrix
Eigen::MatrixXd R; // measurement noise covariance matrix
Eigen::MatrixXd K; // extended kalman gain coefficient
uint64_t timestamp_last; // timestamp of last frame: us
float dt; // delta time: s
public:
Eigen::VectorXd Z; // measurement vector
uint64_t timestamp_now; // timestamp of current frame: us
public:
ExtendedKalmanFilter();
~ExtendedKalmanFilter();
inline bool &IsInited()
{
return is_inited;
}
void Init();
inline void Reset()
{
is_inited = false;
}
inline void SetF(const Eigen::MatrixXd &f)
{
F = f;
}
inline void SetF()
{
F << 1.0f, 0.0f, dt, 0.0f,
0.0f, 1.0f, 0.0f, dt,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f;
}
inline void SetP(const Eigen::MatrixXd &p)
{
P = p;
}
inline void SetQ(const Eigen::MatrixXd &q)
{
Q = q;
}
inline void SetHj(const Eigen::MatrixXd &hj)
{
Hj = hj;
}
inline void SetR(const Eigen::MatrixXd &r)
{
R = r;
}
/**
* @brief Predict step.
*
*/
void Predict();
/**
* @brief Update step.
*
*/
void Update();
};
/**
* @brief To update measurement vector and timestamp.
*
* @param Z0 Z(0)
* @param Z1 Z(1)
* @param Z2 Z(2)
* @param timestamp Current timestamp.
*/
void RecvRawData(float &Z0, float &Z1, float &Z2, uint32_t ×tamp)
{
// Z0 = rho;
// Z1 = phi;
// Z2 = rho_dot;
// timestamp = timestamp_new;
}
#endif
ExtendedKalmanFilter.cpp
#include "math.h"
#include "ExtendedKalmanFilter.h"
/**
* @brief Construct a new Extended Kalman Filter:: Extended Kalman Filter object
*
*/
ExtendedKalmanFilter::ExtendedKalmanFilter()
: is_inited(false),
just_begin_filt(false),
X(4),
F(4, 4),
P(4, 4),
Q(4, 4),
Z(3),
Hj(3, 4),
R(3, 3),
K(4, 3),
timestamp_now(0),
timestamp_last(0),
dt(0.0f)
{
}
/**
* @brief Destroy the Extended Kalman Filter:: Extended Kalman Filter object
*
*/
ExtendedKalmanFilter::~ExtendedKalmanFilter() {}
/**
* @brief Initialize the extended kalman filter.
*
*/
void ExtendedKalmanFilter::Init()
{
if (!IsInited())
{
X << Z(0) * cos(Z(1)),
Z(0) * sin(Z(1)),
Z(2) * cos(Z(1)),
Z(2) * sin(Z(1));
P << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 10.0, 0.0,
0.0, 0.0, 0.0, 10.0;
Q << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;
R << 0.09, 0.0, 0.0,
0.0, 0.009, 0.0,
0.0, 0.0, 0.09;
timestamp_last = timestamp_now;
is_inited = true;
just_begin_filt = true;
}
else
just_begin_filt = false;
}
/**
* @brief Predict step.
*
*/
void ExtendedKalmanFilter::Predict()
{
if (just_begin_filt)
return;
dt = (timestamp_now - timestamp_last) / 1E6;
SetF();
timestamp_last = timestamp_now;
// predict state vector
X = F * X;
// predict state covariance matrix
P = F * P * F.transpose() + Q;
}
/**
* @brief Update step.
*
*/
void ExtendedKalmanFilter::Update()
{
if (just_begin_filt)
return;
float c1 = X(0) ^ 2 + X(1) ^ 2;
float c2 = sqrt(c1);
float c3 = c1 * c2;
if (fabs(c1 > 0.0001))
{
Hj << X(0) / c2, X(1) / c2, 0, 0,
-X(1) / c2, X(0) / c2, 0, 0,
X(1) * (X(2) * X(1) - X(3) * X(0)) / c3, X(0) * (X(3) * X(0) - X(2) * X(1)) / c3, X(0) / c2, X(1) / c2;
}
Eigen::VectorXd(3) hx;
hx << c2, atan2(X(1), X(0)), (X(0) * X(2) + X(1) * X(3)) / c2;
static Eigen::MatrixXd I = Eigen::MatrixXd::Identity(X.size(), X.size());
// calculate extended kalman gain
K = P * Hj.transpose() * (Hj * P * Hj.transpose() + R).inverse();
// update state vector
X = X + K * (Z - hx);
// update state covariance matrix
P = (I - K * Hj) * P;
}
main.cpp
#include "ExtendedKalmanFilter.h"
#include <iostream>
int main()
{
ExtendedKalmanFilter ekf;
while (1)
{
RecvRawData(ekf.Z(0), ekf.Z(1), ekf.Z(2), ekf.timestamp_now);
Init();
Predict();
Update();
}
return 0;
}
点击这里下载完整工程。
6 扩展卡尔曼滤波器的优缺点
优点
扩展卡尔曼滤波与标准卡尔曼滤波有着相似的计算形式，因此运算速度同样很快。
缺点
扩展卡尔曼滤波对非线性的状态转移函数和（或）观测函数使用一阶泰勒级数展开作了线性近似，忽略了二阶及以上的高阶项，因此精度一般（通常称为一阶精度），对于高度非线性问题效果较差。此外，雅可比矩阵的计算较为繁琐，容易出错。
总结
扩展卡尔曼滤波所应用的线性近似是否具有优势主要取决于两个因素：被近似的局部非线性化程度和不确定程度。
7 参考
标签：partial,卡尔曼滤波,py,滤波,贝叶斯,frac,hat,px 来源： https://blog.csdn.net/u012494154/article/details/112518871
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。