自用卡爾曼濾波例子

卡爾曼濾波

參數的確定

example

一個車子在一維空間做一個直線運動，他的加速度滿足高斯分佈，即
a~N(0,V²);
y表示車子在當前時刻的測量得到的一個位置 ，x是當前時刻車子的一個狀態，在這裏是一個向量，$\overline X$_t =$\left[\begin{matrix} x_{t}\\ v_{t} \end{matrix}\right]$ ，其中x_t是當前時刻真實的位置，v_t是當前時刻真實的速度。
根據高中的知識，我們知道
$\begin{cases} x_t=x_{t-1}+a_{t-1}\Delta t+\cfrac{1}{2}+a*t^2 ,&\text {①} \\ v_t=v_{t-1}+a_{t-1}\Delta t ，&\text {②} \end{cases}$
用矩陣來表示的話就是
$\left[\begin{matrix} x_{t}\\ v_{t} \end{matrix}\right]$ =$\left[\begin{matrix} 1 \ \Delta t \\ 0 \ 1 \end{matrix}\right]$ $\left[\begin{matrix} x_{t-1}\\ v_{t-1} \end{matrix}\right]$ +$\left[\begin{matrix} \cfrac12 a(\Delta t)^2 \\ a\Delta t \end{matrix}\right]$
令A=$\left[\begin{matrix} 1 \ \Delta t \\ 0 \ 1 \end{matrix}\right]$ ，那麼這個式子就可以寫作$\overline X$_t =A*$\overline X$_t-1+ $\left[\begin{matrix} \cfrac12 a(\Delta t)^2 \\ a\Delta t \end{matrix}\right]$ ;
可以得到$\overline X$_t 的協方差矩陣是：$V_{\delta}{\over X_t}=E \bigg ((\overline X_t -\mu )*(\overline X_t -\mu )^T\bigg)=E \bigg (\left[\begin{matrix} \cfrac12 a(\Delta t)^2 \\ a\Delta t \end{matrix}\right]*\left[\begin{matrix} \cfrac12 a(\Delta t)^2 \ a\Delta t \end{matrix}\right]\bigg)=E\bigg(\left[\begin{matrix} \cfrac14 a^2(\Delta t)^4 \ \cfrac12 a^2(\Delta t)^3 \\ \cfrac12 a^2(\Delta t)^3 \ a^2(\Delta t)^2 \end{matrix}\right]\bigg)=E(a^2)*\left[\begin{matrix} \cfrac14 (\Delta t)^4 \ \cfrac12 (\Delta t)^3 \\ \cfrac12 (\Delta t)^3 \ (\Delta t)^2 \end{matrix}\right]=V^2*\left[\begin{matrix} \cfrac14 (\Delta t)^4 \ \cfrac12 (\Delta t)^3 \\ \cfrac12 (\Delta t)^3 \ (\Delta t)^2 \end{matrix}\right]$
那麼對應的觀測值y也可以使用類似的式子來表示出來
$y_t=\left[\begin{matrix} 1\ 0\end{matrix}\right]*\left[\begin{matrix} x_{t}\\ v_{t} \end{matrix}\right]+\mu$ 其中$\mu$~ N(0,R)