# 擴展卡曼濾波器

(由擴展卡爾曼濾波器跳轉過嚟)

## 公式

${\displaystyle {\boldsymbol {x}}_{k}=f_{k-1}({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1},{\boldsymbol {w}}_{k-1})}$
${\displaystyle {\boldsymbol {z}}_{k}=h_{k}({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})}$

## 離散時間預測同更新方程

### 預測

 預測狀態估計 ${\displaystyle {\hat {\boldsymbol {x}}}_{k|k-1}=f({\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1},0)}$ 預測協方差估計 ${\displaystyle {\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k}}{\boldsymbol {P}}_{k-1|k-1}{{\boldsymbol {F}}_{k}^{\top }}+{{\boldsymbol {L}}_{k}}{\boldsymbol {Q}}_{k}{{\boldsymbol {L}}_{k}^{\top }}}$

### 更新

 新息或者測量殘差 ${\displaystyle {\tilde {\boldsymbol {y}}}_{k}={\boldsymbol {z}}_{k}-h({\hat {\boldsymbol {x}}}_{k|k-1},0)}$ 新息（或者殘差）協方差 ${\displaystyle {\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{\top }}+{{\boldsymbol {M}}_{k}}{\boldsymbol {R}}_{k}{{\boldsymbol {M}}_{k}^{\top }}}$ 近最優嘅卡曼增益 ${\displaystyle {\boldsymbol {K}}_{k}={\boldsymbol {P}}_{k|k-1}{{\boldsymbol {H}}_{k}^{\top }}{\boldsymbol {S}}_{k}^{-1}}$ 更新唨嘅狀態估計 ${\displaystyle {\hat {\boldsymbol {x}}}_{k|k}={\hat {\boldsymbol {x}}}_{k|k-1}+{\boldsymbol {K}}_{k}{\tilde {\boldsymbol {y}}}_{k}}$ 更新唨嘅協方差估計 ${\displaystyle {\boldsymbol {P}}_{k|k}=({\boldsymbol {I}}-{\boldsymbol {K}}_{k}{{\boldsymbol {H}}_{k}}){\boldsymbol {P}}_{k|k-1}}$

${\displaystyle {{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}}$${\displaystyle {{\boldsymbol {L}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {w}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}}$
${\displaystyle {{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$${\displaystyle {{\boldsymbol {M}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {v}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1})+{\boldsymbol {w}}_{k-1}}$
${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k})+{\boldsymbol {v}}_{k}}$

${\displaystyle {\boldsymbol {L}}_{k},{\boldsymbol {M}}_{k}}$ 就等於 1。

## 普適化

### 連續時間擴展卡曼濾波器

{\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}+\mathbf {v} (t)&\mathbf {v} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {R} (t){\bigr )}\end{aligned}}}

${\displaystyle {\hat {\mathbf {x} }}(t_{0})=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]}{\text{, }}\mathbf {P} (t_{0})=Var{\bigl [}\mathbf {x} (t_{0}){\bigr ]}}$

{\displaystyle {\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}+\mathbf {K} (t){\Bigl (}\mathbf {z} (t)-h{\bigl (}{\hat {\mathbf {x} }}(t){\bigr )}{\Bigr )}\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{\top }-\mathbf {K} (t)\mathbf {H} (t)\mathbf {P} (t)+\mathbf {Q} (t)\\\mathbf {K} (t)&=\mathbf {P} (t)\mathbf {H} (t)^{\top }\mathbf {R} (t)^{-1}\\\mathbf {F} (t)&=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}\\\mathbf {H} (t)&=\left.{\frac {\partial h}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t)}\end{aligned}}}

#### 離散時間測量

{\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} _{k}&=h(\mathbf {x} _{k})+\mathbf {v} _{k}&\mathbf {v} _{k}&\sim {\mathcal {N}}(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}}

${\displaystyle {\hat {\mathbf {x} }}_{0|0}=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\mathbf {P} _{0|0}=E{\bigl [}\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)^{T}{\bigr ]}}$

{\displaystyle {\begin{aligned}{\text{solve }}&{\begin{cases}{\dot {\hat {\mathbf {x} }}}(t)=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}\\{\dot {\mathbf {P} }}(t)=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{\top }+\mathbf {Q} (t)\end{cases}}\qquad {\text{with }}{\begin{cases}{\hat {\mathbf {x} }}(t_{k-1})={\hat {\mathbf {x} }}_{k-1|k-1}\\\mathbf {P} (t_{k-1})=\mathbf {P} _{k-1|k-1}\end{cases}}\\\Rightarrow &{\begin{cases}{\hat {\mathbf {x} }}_{k|k-1}={\hat {\mathbf {x} }}(t_{k})\\\mathbf {P} _{k|k-1}=\mathbf {P} (t_{k})\end{cases}}\end{aligned}}}

${\displaystyle \mathbf {F} (t)=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}}$

${\displaystyle \mathbf {K} _{k}=\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{\top }{\bigl (}\mathbf {H} _{k}\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{\top }+\mathbf {R} _{k}{\bigr )}^{-1}}$
${\displaystyle {\hat {\mathbf {x} }}_{k|k}={\hat {\mathbf {x} }}_{k|k-1}+\mathbf {K} _{k}{\bigl (}\mathbf {z} _{k}-h({\hat {\mathbf {x} }}_{k|k-1}){\bigr )}}$
${\displaystyle \mathbf {P} _{k|k}=(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k})\mathbf {P} _{k|k-1}}$

${\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}}$

### 隱式擴展卡曼濾波器

${\displaystyle h({\boldsymbol {x}}_{k},{\boldsymbol {z'}}_{k})={\boldsymbol {0}}}$

${\displaystyle {{\boldsymbol {R}}_{k}}\leftarrow {{\boldsymbol {J}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {J}}_{k}^{T}}}$
${\displaystyle {\tilde {\boldsymbol {y}}}_{k}\leftarrow -h({\hat {\boldsymbol {x}}}_{k|k-1},{\boldsymbol {z}}_{k})}$

${\displaystyle {{\boldsymbol {J}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {z}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1},{\hat {\boldsymbol {z}}}_{k}}}$

## 考

1. Julier, S.J.; Uhlmann, J.K. (2004). "Unscented filtering and nonlinear estimation" (PDF). Proceedings of the IEEE. 92 (3): 401–422. doi:10.1109/jproc.2003.823141. S2CID 9614092.
2. Courses, E.; Surveys, T. (2006). Sigma-Point Filters: An Overview with Applications to Integrated Navigation and Vision Assisted Control. Nonlinear Statistical Signal Processing Workshop, 2006 IEEE. pp. 201–202. doi:10.1109/NSSPW.2006.4378854. ISBN 978-1-4244-0579-4. S2CID 18535558.
3. Simon, Dan (2006). Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. John Wiley & Sons. ISBN 978-0-471-70858-2.
4. Brown, Robert Grover; Hwang, Patrick Y.C. (1997). Introduction to Random Signals and Applied Kalman Filtering (第3版). New York: John Wiley & Sons. pp. 289–293. ISBN 978-0-471-12839-7.
5. Einicke, G.A. (2019). Smoothing, Filtering and Prediction: Estimating the Past, Present and Future (2nd ed.). Amazon Prime Publishing. ISBN 978-0-6485115-0-2.
6. Quan, Quan (2017). Introduction to multicopter design and control. Singapore: Springer. ISBN 978-981-10-3382-7.
7. Zhang, Zhengyou (1997). "Parameter estimation techniques: a tutorial with application to conic fitting" (PDF). Image and Vision Computing. 15 (1): 59–76. doi:10.1016/s0262-8856(96)01112-2. ISSN 0262-8856.