# 卡曼濾波器

## 過程

### 系統狀態方程

${\displaystyle \mathbf {x} _{t+1}=\mathbf {A} _{t}\mathbf {x} _{t}+\mathbf {B} _{t}\mathbf {u} _{t}+\mathbf {w} _{t}}$
${\displaystyle \mathbf {z} _{t+1}=\mathbf {C} _{t}\mathbf {x} _{t}+\mathbf {v} _{t}}$

${\displaystyle \mathbf {w} _{t},\mathbf {v} _{t}}$分別係過程噪音同埋測量噪音，服從到啲平均值零嘅高斯分佈，即${\textstyle \mathbf {w} _{t}\sim N(0,\mathbf {Q_{t}} ),\mathbf {v} _{t}\sim N(0,\mathbf {R_{t}} )}$

${\displaystyle \mathbf {z} _{t+1}}$ 係輸出值，呢度係觀測模型個測量值。

${\displaystyle \mathbf {A} _{t}}$係系統矩陣，亦即係狀態矩陣；${\displaystyle \mathbf {B} _{t}}$係輸入矩陣；${\displaystyle \mathbf {C} _{t}}$ 係觀測矩陣。

${\displaystyle {\widehat {\mathbf {x} }}_{k+1|k}^{-}=\mathbf {F} _{k}{\widehat {\mathbf {x} }}_{k|k}+\mathbf {B} _{k}\mathbf {u} _{k}}$ （預測方程）
${\displaystyle {\widehat {\mathbf {z} }}_{k+1}=\mathbf {H} _{k+1}{\widehat {\mathbf {x} }}_{k+1|k}^{-}}$ （觀測方程）

### 預測

${\displaystyle {\widehat {\mathbf {x} }}_{k+1|k}^{-}=\mathbf {F} _{k}{\widehat {\mathbf {x} }}_{k|k}+\mathbf {B} _{k}\mathbf {u} _{k}}$
${\displaystyle \mathbf {P} _{k+1|k}^{-}=\mathbf {F} _{k}\mathbf {P} _{k|k}\mathbf {F} _{k}^{\mathrm {T} }+\mathbf {Q} _{k}}$

### 測量同更新

${\displaystyle {\widehat {\mathbf {y} }}_{k+1}=\mathbf {z} _{k+1}-\mathbf {H} _{k+1}{\widehat {\mathbf {x} }}_{k+1|k}^{-}}$
${\displaystyle \mathbf {S} _{k+1}=\mathbf {H} _{k+1}\mathbf {P} _{k+1|k}^{-}\mathbf {H} _{k+1}^{\mathrm {T} }+\mathbf {R} _{k+1}}$

${\displaystyle \mathbf {K} _{k+1}=\mathbf {P} _{k+1|k}^{-}\mathbf {H} _{k+1}\mathbf {S} _{k+1}^{-1}}$

${\displaystyle {\widehat {\mathbf {x} }}_{k+1|k+1}={\widehat {\mathbf {x} }}_{k+1|k}^{-}+\mathbf {K} _{k+1}{\widehat {\mathbf {y} }}_{k+1}}$
${\displaystyle \mathbf {P} _{k+1|k+1}=(\mathbf {I} -\mathbf {K} _{k+1}\mathbf {H} _{k+1})\mathbf {P} _{k+1|k}^{-}}$，或者${\displaystyle \mathbf {P} _{k+1|k+1}=\mathbf {P} _{k+1|k}^{-}-\mathbf {K} _{k+1}\mathbf {S} _{k+1}\mathbf {K} _{k+1}^{\mathrm {T} }}$

### 簡化場景

${\displaystyle {\widehat {\mathbf {y} }}_{k+1}=\mathbf {z} _{k+1}-{\widehat {\mathbf {x} }}_{k+1|k}^{-}}$
${\displaystyle \mathbf {S} _{k+1}=\mathbf {P} _{k+1|k}^{-}+\mathbf {R} _{k+1}}$
${\displaystyle \mathbf {K} _{k+1}=\mathbf {P} _{k+1|k}^{-}\mathbf {S} _{k+1}^{-1}={\dfrac {\mathbf {P} _{k+1|k}^{-}}{\mathbf {P} _{k+1|k}^{-}+\mathbf {R} _{k+1}}}}$
${\displaystyle \mathbf {P} _{k+1|k+1}=\mathbf {P} _{k+1|k}^{-}-\mathbf {K} _{k+1}\mathbf {P} _{k+1|k}^{-}}$

## 校參

${\displaystyle {\widehat {\mathbf {x} }}_{k+1|k+1}={\widehat {\mathbf {x} }}_{k+1|k}^{-}+\mathbf {K} _{k+1}{\widehat {\mathbf {y} }}_{k+1}}$

### 校卡曼增益大細

${\displaystyle \mathbf {K} _{k+1}=\mathbf {P} _{k+1|k}^{-}\mathbf {H} _{k+1}\mathbf {S} _{k+1}^{-1}}$

${\displaystyle \mathbf {K} _{k+1}={\dfrac {\mathbf {F} _{k}\mathbf {P} _{k|k}\mathbf {F} _{k}^{\mathrm {T} }+\mathbf {Q} _{k}}{\mathbf {H} _{k+1}(\mathbf {F} _{k}\mathbf {P} _{k|k}\mathbf {F} _{k}^{\mathrm {T} }+\mathbf {Q} _{k})\mathbf {H} _{k+1}^{\mathrm {T} }+\mathbf {R} _{k+1}}}}$

${\displaystyle \mathbf {K} _{k+1}={\dfrac {\mathbf {P} _{k|k}+\mathbf {Q} _{k}}{\mathbf {P} _{k|k}\mathbf {+} \mathbf {Q} _{k}+\mathbf {R} _{k+1}}}}$

## 考

1. Kalman, R. E. (1960). "A New Approach to Linear Filtering and Prediction Problems". Journal of Basic Engineering. 82: 35–45. doi:10.1115/1.3662552.
2. Humpherys, Jeffrey (2012). "A Fresh Look at the Kalman Filter". Society for Industrial and Applied Mathematics. 54 (4): 801–823. doi:10.1137/100799666.
3. Li, Wangyan; Wang, Zidong; Wei, Guoliang; Ma, Lifeng; Hu, Jun; Ding, Derui (2015). "A Survey on Multisensor Fusion and Consensus Filtering for Sensor Networks". Discrete Dynamics in Nature and Society (英文). 2015: 1–12. doi:10.1155/2015/683701. ISSN 1026-0226.
4. Li, Wangyan; Wang, Zidong; Ho, Daniel W. C.; Wei, Guoliang (2019). "On Boundedness of Error Covariances for Kalman Consensus Filtering Problems". IEEE Transactions on Automatic Control. 65 (6): 2654–2661. doi:10.1109/TAC.2019.2942826. ISSN 0018-9286.
5. Paul Zarchan; Howard Musoff (2000). Fundamentals of Kalman Filtering: A Practical Approach. American Institute of Aeronautics and Astronautics, Incorporated. ISBN 978-1-56347-455-2.
6. Ghysels, Eric; Marcellino, Massimiliano (2018). Applied Economic Forecasting using Time Series Methods. New York, NY: Oxford University Press. p. 419. ISBN 978-0-19-062201-5. OCLC 1010658777.
7. Wolpert, Daniel; Ghahramani, Zoubin (2000). "Computational principles of movement neuroscience". Nature Neuroscience. 3: 1212–7. doi:10.1038/81497. PMID 11127840.