同一般的卡尔曼滤波器(KF)不同的是,扩展卡尔曼滤波器(EKF)是为了解决 在非线性系统中,状态噪声和测量噪声均为高斯分布时,进行最优的状态估计的问题。
- 基本思想:将非线性系统滤波问题近似化为线性系统的滤波问题。具体做法是将非线性的状态方程
f
(
∗
)
f(*)
f(∗)和测量方程
h
(
∗
)
h(*)
h(∗)在状态值
X
k
^
\hat{X_k}
Xk^处进行泰勒展开,并且忽略掉二阶及以上项。转化为线性系统滤波,最后用
Kalman滤波进行状态估计。 - 用途:导弹，卫星，飞机等系统的状态估计问题。
1.局部线性化过程
一般的离散非线性系统大概可以表示成下面的式子:
X
(
k
+
1
)
=
f
[
X
(
k
)
,
k
]
+
G
(
k
)
W
(
k
)
(
1
)
Z
(
k
)
=
h
[
X
(
k
)
,
k
]
+
V
(
k
)
(
2
)
X(k+1) = f[X(k),k]+G(k)W(k) \ \ (1)\\Z(k)=h[X(k),k]+V(k) \ \ (2)
X(k+1)=f[X(k),k]+G(k)W(k) (1)Z(k)=h[X(k),k]+V(k) (2)
其中
W
(
k
)
W(k)
W(k)为状态噪声,
V
(
k
)
V(k)
V(k)为观测噪声,其状态噪声的协方差矩阵为
Q
Q
Q,观测噪声的协方差矩阵为
R
R
R。
对于(1)在先验估计
X
k
^
\hat{X_k}
Xk^处进行一阶泰勒展开:
X
(
k
+
1
)
=
f
(
k
,
X
k
^
)
+
∂
f
∂
X
k
^
(
X
k
−
X
k
^
)
+
G
(
k
)
W
(
k
)
X(k+1)=f(k,\hat{X_k})+\frac{\partial{f}}{\partial{\hat{X_k}}}(X_k-\hat{X_k})+G(k)W(k)
X(k+1)=f(k,Xk^)+∂Xk^∂f(Xk−Xk^)+G(k)W(k)在这里我们设:
Φ
(
k
+
1
∣
k
)
=
∂
f
∂
X
k
^
=
∂
f
∂
X
∣
X
=
X
k
^
ϕ
(
k
)
=
f
(
k
,
X
k
^
)
−
∂
f
∂
X
k
X
k
^
\Phi(k+1|k)=\frac{\partial{f}}{\partial{\hat{X_k}}}=\frac{\partial{f}}{\partial{X}}|_{X=\hat{X_k}}\\ \phi(k)=f(k,\hat{X_k})-\frac{\partial{f}}{\partial{X_k}}\hat{X_k}
Φ(k+1∣k)=∂Xk^∂f=∂X∂f∣X=Xk^ϕ(k)=f(k,Xk^)−∂Xk∂fXk^因此可以将状态方程化为:
X
(
k
+
1
)
=
Φ
(
k
+
1
∣
k
)
X
(
k
)
+
ϕ
(
k
)
+
G
(
k
)
W
(
k
)
X(k+1)=\Phi(k+1|k)X(k)+\phi(k)+G(k)W(k)
X(k+1)=Φ(k+1∣k)X(k)+ϕ(k)+G(k)W(k)其中
Φ
(
k
+
1
∣
k
)
\Phi(k+1|k)
Φ(k+1∣k)被称为状态转移矩阵:
Φ
(
k
+
1
∣
k
)
=
∂
f
∂
X
k
^
=
(
∂
f
1
∂
x
1
∂
f
1
∂
x
2
∂
f
1
∂
x
3
.
.
.
∂
f
1
∂
x
n
∂
f
2
∂
x
1
∂
f
2
∂
x
2
∂
f
2
∂
x
3
.
.
.
∂
f
2
∂
x
n
.
.
.
∂
f
n
∂
x
1
∂
f
n
∂
x
2
∂
f
n
∂
x
3
.
.
.
∂
f
n
∂
x
n
)
∣
X
=
X
k
^
\Phi(k+1|k)=\frac{\partial{f}}{\partial{\hat{X_k}}}=\begin{pmatrix} \frac{\partial{f_1}}{\partial{x_1}} & \frac{\partial{f_1}}{\partial{x_2}} & \frac{\partial{f_1}}{\partial{x_3}}...& \frac{\partial{f_1}}{\partial{x_n}}\\ \frac{\partial{f_2}}{\partial{x_1}} & \frac{\partial{f_2}}{\partial{x_2}} & \frac{\partial{f_2}}{\partial{x_3}}...& \frac{\partial{f_2}}{\partial{x_n}} \\...\\ \frac{\partial{f_n}}{\partial{x_1}} & \frac{\partial{f_n}}{\partial{x_2}} & \frac{\partial{f_n}}{\partial{x_3}}...& \frac{\partial{f_n}}{\partial{x_n}} \end{pmatrix}|_{X=\hat{X_k}}
Φ(k+1∣k)=∂Xk^∂f=⎝⎜⎜⎜⎛∂x1∂f1∂x1∂f2...∂x1∂fn∂x2∂f1∂x2∂f2∂x2∂fn∂x3∂f1...∂x3∂f2...∂x3∂fn...∂xn∂f1∂xn∂f2∂xn∂fn⎠⎟⎟⎟⎞∣X=Xk^同样的,将(2)在
X
k
^
\hat{X_k}
Xk^处同样可以进行一阶泰勒展开,得到:
Z
(
k
)
=
H
(
k
)
X
(
k
)
+
y
(
k
)
+
V
(
k
)
Z(k)=H(k)X(k)+y(k)+V(k)
Z(k)=H(k)X(k)+y(k)+V(k)其中:
H
(
k
)
=
∂
h
∂
X
k
^
=
(
∂
h
1
∂
x
1
∂
h
1
∂
x
2
∂
h
1
∂
x
3
.
.
.
∂
h
1
∂
x
n
∂
h
2
∂
x
1
∂
h
2
∂
x
2
∂
h
2
∂
x
3
.
.
.
∂
h
2
∂
x
n
.
.
.
∂
h
n
∂
x
1
∂
h
n
∂
x
2
∂
h
n
∂
x
3
.
.
.
∂
h
n
∂
x
n
)
∣
X
=
X
k
^
H(k)=\frac{\partial{h}}{\partial{\hat{X_k}}}=\begin{pmatrix} \frac{\partial{h_1}}{\partial{x_1}} & \frac{\partial{h_1}}{\partial{x_2}} & \frac{\partial{h_1}}{\partial{x_3}}...& \frac{\partial{h_1}}{\partial{x_n}}\\ \frac{\partial{h_2}}{\partial{x_1}} & \frac{\partial{h_2}}{\partial{x_2}} & \frac{\partial{h_2}}{\partial{x_3}}...& \frac{\partial{h_2}}{\partial{x_n}} \\...\\ \frac{\partial{h_n}}{\partial{x_1}} & \frac{\partial{h_n}}{\partial{x_2}} & \frac{\partial{h_n}}{\partial{x_3}}...& \frac{\partial{h_n}}{\partial{x_n}} \end{pmatrix}|_{X=\hat{X_k}}
H(k)=∂Xk^∂h=⎝⎜⎜⎛∂x1∂h1∂x1∂h2...∂x1∂hn∂x2∂h1∂x2∂h2∂x2∂hn∂x3∂h1...∂x3∂h2...∂x3∂hn...∂xn∂h1∂xn∂h2∂xn∂hn⎠⎟⎟⎞∣X=Xk^
2.线性kalman滤波
将非线性的方程转化成线性之后就可以进行kalman滤波了:
X
^
k
−
=
f
(
X
^
k
−
1
)
P
^
k
−
=
Φ
(
k
∣
k
−
1
)
P
^
k
Φ
T
(
k
∣
k
−
1
)
+
G
k
−
Q
k
G
k
−
T
K
k
=
P
^
k
−
H
k
T
(
H
k
P
^
k
−
H
k
T
+
R
k
)
−
1
X
^
k
=
X
^
k
−
+
K
k
(
z
k
−
h
(
X
^
k
−
)
)
P
k
^
=
(
I
−
K
k
H
k
)
P
^
k
−
\hat{X}_{k}^-=f(\hat{X}_{k-1})\\ \hat{P}_k^-=\Phi(k|k-1)\hat{P}_k\Phi^T(k|k-1)+G_{k}^-Q_kG_{k}^{-T}\\ K_k=\hat{P}_k^-H_k^T(H_k\hat{P}_k^-H_k^T+R_k)^{-1} \\ \hat{X}_k=\hat{X}_{k}^-+K_k(z_k-h(\hat{X}_k^-)) \\ \hat{P_k}=(I-K_kH_k)\hat{P}_k^-
X^k−=f(X^k−1)P^k−=Φ(k∣k−1)P^kΦT(k∣k−1)+Gk−QkGk−TKk=P^k−HkT(HkP^k−HkT+Rk)−1X^k=X^k−+Kk(zk−h(X^k−))Pk^=(I−KkHk)P^k−以上就是其推导的总过程。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
