文章目录
- 1. 线性系统的 Kalman Filter 回顾
- 2. Extended Kalman Filter 之 DR_CAN讲解笔记
- 2.1. 非线性系统
- 2.2. 非线性系统线性化
- 2.3. Extended Kalman Filter
- 3. Extended Kalman Filter 之 我的理解与详细解释
1. 线性系统的 Kalman Filter 回顾
-
系统状态空间方程：
x k = A x k − 1 + B U k − 1 + w k − 1 P ( ω ) ∼ N ( 0 , Q ) z k = H x k + H k P ( v ) ∼ N ( 0 , R ) \begin{array}{ll} x_k=A x_{k-1}+B U_{k-1}+w_{k-1} & P(\omega) \sim N(0, Q) \\ z_k=H x_k+H_k & P(v) \sim N(0, R) \end{array} xk=Axk−1+BUk−1+wk−1zk=Hxk+HkP(ω)∼N(0,Q)P(v)∼N(0,R) -
预测：
x ^ k − = A x ^ k − 1 + B u k − 1 P k − = A P k − 1 A ⊤ + Q \begin{aligned} & \hat{x}_k^{-}=A \hat{x}_{k-1}+B u_{k-1} \\ & P_k^{-}=A P_{k-1} A^{\top}+Q \end{aligned} x^k−=Ax^k−1+Buk−1Pk−=APk−1A⊤+Q -
矫正（更新）：
K k = P k − H ⊤ H P k − H ⊤ + R Y ^ k = X ^ k − + K k ( Z k − H X ^ k − ) P k = ( I − K k H ) P k − \begin{aligned} & K_k=\frac{P_k^{-} H^{\top}}{H P_k^{-} H^{\top}+R} \\ & \hat{Y}_k=\hat{X}_k^{-}+K_k\left(Z_k-H \hat{X}_k^{-}\right) \\ & P_k=\left(I-K_k H\right) P_k^{-} \end{aligned} Kk=HPk−H⊤+RPk−H⊤Y^k=X^k−+Kk(Zk−HX^k−)Pk=(I−KkH)Pk−
2. Extended Kalman Filter 之 DR_CAN讲解笔记
2.1. 非线性系统
对于非线性系统来说，系统数学模型和观测模型无法用状态空间方程来表达，因为状态空间方程是线性方程，而非线性系统的这两个模型都是非线性的。可以写成如下形式：
x
k
=
f
(
x
k
−
1
,
u
k
−
1
,
ω
k
−
1
)
P
(
ω
)
∼
N
(
0
,
Q
)
z
k
=
h
(
x
k
,
v
k
)
P
(
v
)
∼
N
(
0
,
R
)
\begin{aligned} & x_k=f\left(x_{k-1}, u_{k-1}, \omega_{k-1}\right) & P(\omega) \sim N(0, Q)\\ & z_k=h\left(x_k, v_k\right) & P(v) \sim N(0, R) \end{aligned}
xk=f(xk−1,uk−1,ωk−1)zk=h(xk,vk)P(ω)∼N(0,Q)P(v)∼N(0,R)
注意：上述的高斯噪声是在非线性函数里面的，这样即使噪声原本是高斯的，但是经过非线性系统之后，它的分布也不再是高斯分布了，如下图所示。
可以发现，Kalman Filter的两个前提条件：（1）线性系统；（2）高斯噪声，已经全都不满足了。
所以对于非线性系统应用 Kalman Filter 的最好方法就是对系统在工作点/线性化点 进行线性化，因为只有线性化之后我们才能写成线性的形式，才能得到系数矩阵 A A A和 H H H，进而计算Kalman Gain，最后进行数据融合。
2.2. 非线性系统线性化
线性化需要在一个线性化点进行，对于非线性系统的Kalman Filter来说，最准确的地方就是在真实状态点进行线性化。
但是真实状态点到底是多少我们永远都不知道，因为如果知道了就不需要做 Kalman Filter了。所以我们只能在我们知道的工作点处进行线性化，我们知道的工作点有两个：
- 上一次的最优估计状态 x ^ k − 1 \hat{x}_{k-1} x^k−1
- 这一次的先验估计状态 x ^ k − \hat{x}_k^- x^k−
2.2.1. 状态方程 f ( x k ) f(x_k) f(xk)在上一次的最优估计状态 x ^ k − 1 \hat{x}_{k-1} x^k−1处线性化
2.2.2. 观测方程 h ( x k ) h(x_k) h(xk)在这一次的预测状态 x ~ k \tilde{x}_k x~k处线性化
2.3. Extended Kalman Filter
3. Extended Kalman Filter 之 我的理解与详细解释
既然是卡尔曼滤波，自然就是两个方程：状态方程和观测方程。由于卡尔曼滤波是离散的，所以下面我们先给出IMU的离散状态空间方程和状态观测方程如下：
x k = f ( x k − 1 , w k − 1 ) z k = h ( x k , v k ) \boldsymbol x_{k} = \boldsymbol f(\boldsymbol x_{k-1} , \boldsymbol w_{k-1}) \\ \boldsymbol z_{k} = \boldsymbol h(\boldsymbol x_{k}, \boldsymbol v_{k} ) \\ xk=f(xk−1,wk−1)zk=h(xk,vk)
由于这两个方程都是非线性方程，所以为了使用使用卡尔曼滤波，必须对他们进行线性化。并且由于线性化必须知道线性化工作点，而我们实际知道的都是名义状态值，这里的名义值包括 上一次的状态后验值 x ˇ k − 1 \boldsymbol {\check{x}}_{k-1} xˇk−1 和 这一次的状态先验值 x ^ k \boldsymbol {\hat{x}}_{k} x^k。所以对上面两个公式进行线性化也都是在 名义状态 的地方进行线性化，得到如下的公式：
x ^ k = f ( x ^ k − 1 , 0 ) + F k − 1 ( x k − 1 − x ˇ k − 1 ) + W k − 1 w k − 1 z k = h ( x ^ k , 0 ) + H k ( x k − x ^ k ) + V k v k \boldsymbol {\hat{x}}_{k} = \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) + \boldsymbol F_{k-1} (\boldsymbol x_{k-1} - \boldsymbol {\check{x}}_{k-1}) + \boldsymbol W_{k-1} \boldsymbol w_{k-1} \\ \boldsymbol z_{k} = \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) + \boldsymbol H_{k} (\boldsymbol x_{k} - \boldsymbol {\hat{x}}_{k}) + \boldsymbol V_{k} \boldsymbol v_{k} x^k=f(x^k−1,0)+Fk−1(xk−1−xˇk−1)+Wk−1wk−1zk=h(x^k,0)+Hk(xk−x^k)+Vkvk
注意：
（1） 时刻记住我们线性化的关键目的是什么：是为了得到系数矩阵，从而变成线性系统，可以使用典型的线性系统卡尔曼滤波。
（2） 上面的方程线性化出来的两个固定函数值 f ( x ^ k − 1 , 0 ) \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) f(x^k−1,0) 和 h ( x ^ k , 0 ) \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) h(x^k,0) 对我们使用卡尔曼滤波没有影响，因为我们计算协方差矩阵的时候是使用系数矩阵来计算的。
（3）假设传感器观测到的值是
z
m
\boldsymbol z_m
zm，这里省略掉了时间下标
k
k
k，然后用下标
m
m
m表示是 观测的测量值。然后用
z
\boldsymbol z
z代表预测观测值，也就是我们把IMU预测得到的状态（先验状态）
x
^
k
\boldsymbol {\hat{x}}_{k}
x^k 带入到观测方程中得到的 计算出来的观测值。因为看上面的典型的卡尔曼滤波公式就知道，我们是把IMU 先验状态 带入观测方程中得到一个 计算出来的预测观测值，然后和真正的观测传感器的测量值作差，再乘以卡尔曼增益进行校正。所以这里计算的 预测观测值 表达公式如下（因为我们在计算，噪声不知道是多少，所以简化为0）：
z
=
h
(
x
^
k
,
0
)
+
H
k
(
x
^
k
−
x
^
k
)
+
V
k
0
=
h
(
x
^
k
,
0
)
\boldsymbol z = \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) + \boldsymbol H_{k} (\boldsymbol {\hat{x}}_{k} - \boldsymbol {\hat{x}}_{k}) + \boldsymbol V_{k} \boldsymbol 0 = \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)
z=h(x^k,0)+Hk(x^k−x^k)+Vk0=h(x^k,0)
（4）重要：上面的公式中， x k − 1 \boldsymbol {x}_{k-1} xk−1 和 x k \boldsymbol {x}_{k} xk 是 自变量， x ^ k \boldsymbol {\hat x}_k x^k 和 z k \boldsymbol z_{k} zk 是 函数值，所以我们带入不同的自变量值会得到不同的函数值。比如我们实际计算的时候，在预测方程中带入的是 上一次的后验状态值 x ˇ k − 1 \boldsymbol {\check{x}}_{k-1} xˇk−1，那么得到的就是 本次的先验状态值 x ^ k = f ( x ^ k − 1 , 0 ) \boldsymbol{\hat{x}}_{k} = \boldsymbol f(\boldsymbol {\hat{x}}_{k-1}, \boldsymbol 0) x^k=f(x^k−1,0)；我们在观测方程中带入 本次的先验状态值，得到的就是 本次的先验预测观测值 z k = h ( x ^ k , 0 ) \boldsymbol z_{k} = \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) zk=h(x^k,0)。
（5）注意：在我们计算卡尔曼滤波的时候，我们只能按照（4）中所说的那样带入 上一次的先验状态到状态方程，然后带入 这一次的先验状态到观测方程。因为在典型的线性卡尔曼滤波中就是这么做的，我们就是要融合状态方程和观测方程。
最后，给出EKF的卡尔曼滤波方程为：
预测公式：
带入上一时刻的后验状态到自变量中，即
x
k
−
1
←
x
ˇ
k
−
1
:
x
^
k
=
f
(
x
^
k
−
1
,
0
)
+
F
k
−
1
(
x
ˇ
k
−
1
−
x
ˇ
k
−
1
)
=
f
(
x
^
k
−
1
,
0
)
P
^
k
=
F
k
−
1
P
ˇ
k
−
1
F
k
−
1
T
+
Q
带入上一时刻的后验状态到自变量中，即\boldsymbol x_{k-1} \leftarrow \boldsymbol {\check{x}}_{k-1} : \\\boldsymbol {\hat{x}}_{k} = \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) + \boldsymbol F_{k-1} (\boldsymbol {\check{x}}_{k-1} - \boldsymbol {\check{x}}_{k-1}) = \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) \\ \ \boldsymbol {\hat P}_{k} = \boldsymbol F_{k-1} \boldsymbol {\check P}_{k-1} \boldsymbol F_{k-1} ^{T}+ \boldsymbol Q
带入上一时刻的后验状态到自变量中，即xk−1←xˇk−1:x^k=f(x^k−1,0)+Fk−1(xˇk−1−xˇk−1)=f(x^k−1,0) P^k=Fk−1Pˇk−1Fk−1T+Q
校正公式：
带入这一时刻的先验状态到自变量中，即
x
k
←
x
^
k
:
z
k
=
h
(
x
^
k
,
0
)
+
H
k
(
x
^
k
−
x
^
k
)
=
h
(
x
^
k
,
0
)
K
k
=
P
^
k
H
T
H
P
^
k
H
T
+
R
x
ˇ
k
=
x
^
k
+
K
k
(
z
m
−
h
(
x
^
k
,
0
)
)
P
ˇ
k
=
(
I
−
K
k
H
)
P
^
k
带入这一时刻的先验状态到自变量中，即\boldsymbol x_{k} \leftarrow \boldsymbol {\hat{x}}_{k} : \\ \boldsymbol z_{k} = \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) + \boldsymbol H_{k} (\boldsymbol {\hat{x}}_{k} - \boldsymbol {\hat{x}}_{k}) = \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \\ \begin{gathered} \boldsymbol K_{k}=\frac{ \boldsymbol {\hat P}_{k} \boldsymbol H^{T}}{ \boldsymbol H \boldsymbol {\hat P}_{k} \boldsymbol H^{T} + \boldsymbol R} \\ \boldsymbol {\check {x}}_{k} = \boldsymbol {\hat{x}}_{k} + \boldsymbol {K}_{k}\left( \boldsymbol {z}_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \right) \\ \boldsymbol {\check P}_{k}=\left( \boldsymbol I- \boldsymbol K_{k} \boldsymbol H\right) \boldsymbol {\hat P}_{k} \end{gathered}
带入这一时刻的先验状态到自变量中，即xk←x^k:zk=h(x^k,0)+Hk(x^k−x^k)=h(x^k,0)Kk=HP^kHT+RP^kHTxˇk=x^k+Kk(zm−h(x^k,0))Pˇk=(I−KkH)P^k
