对卡尔曼滤波的理解

__________________2018.1.17 全部更新完毕 ♥ 转载请与作者联系_________________

(课程作业code见GitHub) 由于水平有限，自己写的一些简单代码可能存在问题，还希望读者给出批评指正。

提到卡尔曼滤波，要有一个宏观的概念。
在估计与滤波这门课上我接触到了卡尔曼滤波，首先，整体的思想是，我是用来做估计的。
给出动态系统的两个方程（Consider the following dynamic equation）：

　　　　 $x_{k+1}=F_kx_k+G_ku_k+w_k　　　 k=0,1,...$
　　　　
where $w_k , k=0,1,...$,is a white Gaussian, possibly non-stationary, sequence of zero-mean with covariance
　　　　　　　　　　　　 $E[w_kw_k']＝Q_k$
　　　　　　　　　　　　
The measurement equation is
　　　　　　　　　　　　$z_k=H_kx_k+v_k$
　　　　　　　　　　　　
where $v_k$ is a zero-mean white Gaussian, possibly non-stationary, sequence of covariance
　　　　　　　　　　　　$E[v_kv_k']=R_k$　
　　　　　　　　　　　　
It is assumed that
　　　　　　　　　　　　$x_0 \sim \mathscr N(\bar x_0,P_0)$
　　　　　　　　　　　　
Also, the two noise sequences and $x_0$ are assumed mutually independent.
The above constitutes the linear Gaussian (LG) assumption.
What is optimal state estimation problem?——Estimate $x_j$ given $z^k≜\{z_i,i\le k \}$ under some optimality criterion.

根据以上给出的条件，我们知道了卡尔曼滤波所要做的工作——“估计（Estimate）”，根据系统状态和数据给出的时刻的不同又要分为三种具体的情况考虑。
Three types of state estimations problems:

• filtering if $j=k$
• smoothing if $j<k$
• prediction if $j>k$

这里，$j$是状态的时刻，$k$是测量数据的时刻。并且$j,k$在上标（如$z^k$）是一种标记，意味着一个set（集合$z^k≜\{z_i,i\le k \}$）。 在下标意味着某一时刻的一个值。
简单来说，有当前时刻的状态和当前时刻的数据时，意味着进行滤波；有前一时刻的状态和当前时刻的数据，意味着平滑；有当前时刻的状态而数据是上一时刻的，意味着预测。

Under the above linear Gaussian formulation, the MMSE (filtering) estimate of the state is
　　　　　　　　　　$\hat x_{k|k}=E[x_k|z^k]$　
【how to get mean? 使用了条件均值，求均值意味着求积分】

with the associated MSE matrix
　　　　　$P_{k|k}=MSE(\hat x_{k|k}|z^k)=E[(x_k-\hat x_{k|k} )'|z^k]=cov(x_k|z^k)$
【给出了条件协方差，即真实状态的函数】

这里提到MMSE作为度量优化准则即回答 how about the result的问题。MMSE容易得到mean， 比MAP使用density 计算简单。MSE矩阵给出了how sure you are, 即回答你的估计有多可靠。

【卡尔曼滤波与之前学过的MMSE和MSE对比】

The Kalman filter provides a recursive implementation of this in that within one cycle, $\hat x_{k|k}$ and $P_{k|k}$ are evolved into $\hat x_{k+1|k+1}$ and $P_{k+1|k+1}$.

Recall that if two random vectors $x$ and $z$ are jointly Gaussian and $z$ is an observation about the unknown parameter $x$ ，one has

　　　　　　$\hat x^{MMSE}=E[x|z]=\bar x + C_{xz}C_z^{-1}(z-\bar z)$
　　　　　　
　　　　　　$P=MSE(\hat x^{MMSE}|z)=C_x-C_{xz}C_z^{-1}C_{zx}$
【此处是卡尔曼滤波的推理开始，当我们得到了k时刻的状态参数和其最优估计值，当得到k+1时刻的数据之后我们就可以得到k+1时刻的状态参数估计值以及k+1时刻的数据估计。】

By treating dynamic estimation as a recursive static estimation, the Kalman filter can then be abtained with the following replacement:
$x \rightarrow x_{k+1}$
$\bar x \rightarrow \hat x_{k+1|k} = E[x_{k+1}|z^k]$
$z \rightarrow z_{k+1}$
$\bar z \rightarrow \hat z_{k+1|k}=E[z_{k+1}|z^{k}]$
$\hat x \rightarrow \hat x_{k+1|k+1} = E[x_{k+1}|z^{k+1}]$
$C_x \rightarrow P_{k+1|k} = cov(x_{k+1}|z^k)=cov(\tilde x_{k+1|k}|z^k )=MSE(\hat x_{k+1|k}|z^k)$
$C_z \rightarrow S_{k+1}=cov(z_{k+1}|z^k)=cov(\tilde z_{k+1|k}|z^k )$
$C_{xz} \rightarrow cov(x_{k+1},z_{k+1}|z^k)=cov(\tilde x_{k+1|k},\tilde z_{k+1|k}|z^k)$
$P \rightarrow P_{k+1|k+1} = cov(x_{k+1}|z^{k+1})=cov(\tilde x_{k+1|k+1}|z^{k+1}) = MSE(\hat x_{k+1|k+1}|z^{k+1})$

where

$\tilde x_{k|k}=x_k -\hat x_{k|k}$
$\tilde x_{k+1|k} = x_{k+1}-\hat x_{k+1|k}$
$\tilde z_{k+1|k} = z_{k+1}-\hat z_{k+1|k}$

代入状态方程与量测方程数据
$\hat x_{k+1|k}= E[x_{k+1}|z^k]=E[F_kx_k+G_ku_k+w_k|z_k]=F_k\hat x_{k|k}+G_ku_k$
$\tilde x_{k+1|k} = x_{k+1} - \hat x_{k+1|k} = F_k\tilde x_{k|k}+w_k$
$P_{k+1|k}=E[\tilde x_{k+1|k}\tilde x_{k+1|k}'|z^k]=F_kP_{k|k}F_k' +Q_k$
$\hat z_{k+1|k}=E[z_{k+1}|z^k]=E[H_{k+1}x_{k+1}+v_{k+1}|z^k]=H_{k+1}\hat x_{k+1|k}$
$\tilde z_{k+1|k}=z_{k+1}- \hat z_{k+1|k}=H_{k+1}\tilde x_{k+1}+v_{k+1}$
$S_{k+1}=E[\tilde z_{k+1|k}\tilde z_{k+1|k}'|z^k]=H_{k+1}P_{k+1|k}H_{k+1}' +R_{k+1}$
$cov(x_{k+1,z_{k+1}|z^k})=E[\tilde x_{k+1}|\tilde z _{k+1}|z^k]=P_{k+1|k}H_{k+1}'$

Define
$K_{k+1} = cov(x_{k+1,z_{k+1}|z^k})S_{k+1}^{-1}=P_{k+1|k}H_{k+1}'S_{k+1}^{-1}$

Then
$\hat x_{k+1|k+1} = \hat x_{k+1|k}+K_{k+1} \tilde z_{k+1|k}$
$P_{k+1|k+1}=P_{k+1|k}-P_{k+1|k}H_{k+1}'S_{k+1}^{-1}H_{k+1}P_{k+1|k}$
$=(I-k_{k+1}H_{k+1})P_{k+1|k} ...(1)$
$=P_{k+1|k}-K_{k+1}S_{k+1}K_{k+1}' ...(2)$

An alternative form, which holds for an arbitrary gain matrix $K_{k+1}$ called the Joseph form, is

$P_{k+1|k+1} = (I -K_{k+1}H_{k+1})P_{k+1|k}(I - K_{k+1}H_{k+1})'+ K_{k+1}R_{k+1}K_{k+1}' ...(3)$

(3)式的优点：既保持了矩阵的对称性，也保证了矩阵为非负定矩阵。

KF 最终使用形式

状态和测量的预测值（time update）:
$\hat x _{k+1|k} = F_k \hat x_{k|k} + G_ku_k$
$P_{k+1|k}=F_kP_{k|k}F_k' +Q_k$
$\hat z_{k+1|k}=H_{k+1} \hat x_{k+1|k}$
$S_{k+1}=H_{k+1}P_{k+!|k}H_{k+1}' +R _{k+1}$

状态更新（measurement update）:
$K_{k+1}=P_{k+1|k}H_{k+1}'S_{k+1}^{-1}$
$\hat x_{k+1|k+1} = \hat x_{k+1|k}+K_{k+1}(z_{k+1}-\hat z_{k+1|k})$
$P_{k+1|k+1} = P_{k+1|k}-K_{k+1}S_{k+1}K_{k+1}'$

迭代形式需要初始化， $\hat x_{0|0}=E[x_0|z^0]=\bar x_0 of x_0$,and the associated MSE initial MSE $P_{0|0}=MSE(\hat x_{0|0}|z^0) =cov(x_0|z^0)=P_0$