本文将简单推导卡尔曼滤波器的预测和更新公式。为了简单,使用标量(一维向量)而不是向量,并且假设系统没有输入。
系统状态的理论值如下:
$x_k=\Phi_k x_{k-1}+w_k$
但是由于过程噪声和观测噪声的存在,系统状态的真实值是不可知的。但我们仍可以根据以下思路,尽量跟踪真实值:
1) 状态转换系数是已知的,因此我们可以根据上一状态得到当前状态的先验估计:
$\hat{x}_k^-=\Phi_k \hat{x}_{k-1}$
2) 将对当前状态的先验估计$\hat{x}_k^-$变换到对输出的先验估计$H\hat{x}_k^-$,求出观测值和先验估计值之间的差:
$\tilde{y}_k=z_k-H\hat{x}_k^-$
$=z_k-H\Phi\hat{x}_{k-1}$
3) 将上一步的差乘上一个系数,作为对先验估计的修正,得到当前状态的后验估计:
$\hat{x}_k=\Phi_k\hat{x}_{k-1}+K_k(z_k-H\Phi_k\hat{x}_{k-1})$
我们的目标是求出上式的系数$K_k$,以使得误差$\tilde{x}_k=x_k-\hat{x}_k$的方差最小。
观测值可以用表示为真实输出与观测噪声的叠加:
$z_k=Hx_k+v_k$
于是误差$\tilde{x}_k$可以表示为如下形式(消去了$z_k$和$x_k^-$):
$\tilde{x}_k=x_k-\Phi\hat{x}_{k-1}-K_k(Hx_k+v_k-H\Phi_k\hat{x}_{k-1})$
还可以进一步消去$x_k$:
$\tilde{x}_k=\Phi_k x_{k-1} + w_k - \Phi_k \hat{x}_{k-1} - K_k(H\Phi_k x_{k-1} + Hw_k +v_k - H\Phi_k \hat{x}_{k-1})$
上式中,项$x_{k-1}$和$\hat{x}_{k-1}$可以合并,并使用下式进行替换:
$\tilde{x}_{k-1}=x_{k-1}-\hat{x}_{k-1}$
于是我们得到:
$\tilde{x}_k=(1-K_kH)\tilde{x}_{k-1}\Phi_k+(1-K_kH)w_k-K_kv_k$
这样我们就得到了$\tilde{x}_k$递推表达式。