卡尔曼滤波五个公式推导

一、两个方程

1，系统的状态方程（预测方程）
$$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$$
2，测量方程
$$z_k=H_k{x}_k+v_k$$
其中：

​ $x_k$是状态向量；

​ $A_k$是状态转移矩阵；

​ $u_k$是状态控制向量；

​ $B_k$是控制变量矩阵；

​ $w_k$是控制系统的噪声，服从高斯分布：$w_k\sim N(0,Q)$

​ $Q$是系统噪声协方差矩阵；

​ $z_k$是测量向量；

​ $H_k$是状态向量到测量向量的转换矩阵；

​ $v_k$是测量的噪声，服从高斯分布：$v_k\sim N(0,R)$

​ $R$是测量噪声的协方差矩阵。

二、五个公式

预测：
$$\begin{gathered} {\hat{x}}_{\bar{k}}=A_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_{\bar{k}}=A_k{P}_{k-1}{A}_k^T+Q \end{gathered}$$
更新：
$$\begin{gathered} K_k=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R} \\ {\hat{x}}_k={\hat{x}}_{\bar{k}}+K_k(z_k-H_k{\hat{x}}_{\bar{k}}) \\ {P}_k=(I-K_k{H}_k)P_{\bar{k}} \end{gathered}$$
其中：

1. ${\hat{x}}_{k-1}$和${\hat{x}}_k$，分别为k-1时刻与k时刻的后验状态估计，是滤波结果之一，即更新后的结果，也叫最优估计。（因为不可能知道真实值，所以加一个hat，表示最优估计）
2. ${\hat{x}}_{\bar{k}}$，表示k时刻的的先验状态估计，及根据k-1时刻的最优估计预测k时刻的状态。
3. $P_{k-1}$和$P_k$，分别为k-1时刻与k时刻的后验估计协方差（即${\hat{x}}_{k-1}$和${\hat{x}}_k$的协方差，表示状态的不确定性），为滤波结果之一。
4. $P_{\bar{k}}$，为k时刻的先验估计协方差（${\hat{x}}_{\bar{k}}$的协方差）。
5. $K_k$，为卡尔曼增益。

三、五个公式的推导过程

1，预测

定理1：随机变量函数的分布(高斯分布)

已知$X$服从高斯分布：$X\sim N(\mu ,{\sigma }^2)$，且$Y=aX+b$。则$Y$服从高斯分布：$Y\sim N(a\mu +b,a^2{\sigma }^2)$

已知k-1时刻系统的最优状态服从高斯分布：${\hat{X}}_{k-1}\sim N({\hat{x}}_{k-1},P_{k-1})$

根据预测方程：$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$

及上面的定理，可以直接推导出k时刻的预测状态，服从高斯分布：

${\hat{X}}_{\bar{k}}\sim N(A_k{\hat{x}}_{k-1}+B_k{u}_k,A_k{P}_{k-1}{A}_k^T+Q)$

从而得到k时刻的预测状态：
$$\begin{gathered} {\hat{x}}_{\bar{k}}=A_k{\hat{x}}_{k-1}+B_k{u}_k \\ {P}_{\bar{k}}=A_k{P}_{k-1}{A}_k^T+Q \end{gathered}$$
PS1：因为是根据k-1时刻预测得到的k时刻的状态，因此在变量上加了个-，表示k时刻的先验估计。

PS2：这里的变量及参数都是矩阵的形式，因此k-1时刻的协方差矩阵$P_{k-1}$的变换形式为$A_k{P}_{k-1}{A}_k^T$。

2，更新

定理2：两个高斯分布的乘积依然为高斯分布

已知$X$服从高斯分布：$X\sim N({\mu }_1,{\sigma }_1^2)$，$Y$服从高斯分布：$Y\sim N({\mu }_2,{\sigma }_2^2)$，则$XY$服从高斯分布：$XY\sim N(\mu ,{\sigma }^2)$
$$\begin{gathered} K=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2} \\ \mu ={\mu }_1+K({\mu }_2-{\mu }_1) \\ {\sigma }^2=(1-K){\sigma }_1^2 \end{gathered}$$

由上面知道k时刻的预测状态服从高斯分布，${\hat{X}}_{\bar{k}}\sim N({\hat{x}}_{\bar{k}},P_{\bar{k}})$

且由测量方程：$z_k=H_k{x}_k+v_k$

按照定理1进行转换，即将k时刻的预测状态转换到k时刻的测量状态，同样服从高斯分布：${\hat{Z}}_{\bar{k}}\sim N(H_k{\hat{x}}_{\bar{k}},H_k{P}_{\bar{k}}{H}_k^T)$。

注意：这里协方差矩阵$H_k{P}_{\bar{k}}{H}_k^T$没有$+R$是因为这里只是状态的转换，并不包含实际的测量。

测量本身服从高斯分布：$Z_k\sim N(z_k,R)$

我们对上面两个高斯分布进行相乘，即k时刻预测的乘以k时刻测量的，便得到k时刻状态的最优估计：

${\hat{X}}_k\sim N({\hat{x}}_k,P_k)$

这里需要注意的是，我们同样需要把k时刻状态的最优估计转换到测量状态下：${\hat{Z}}_k\sim N(H_k{\hat{x}}_k,H_k{P}_k{H}_k^T)$。

按照定理2：${\hat{Z}}_{\bar{k}}\times Z_k={\hat{Z}}_k$
$$\begin{gathered} K^{\prime }=\frac{H_k{P}_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R} \\ {H}_k{\hat{x}}_k=H_k{\hat{x}}_{\bar{k}}+K^{\prime }(z_k-H_k{\hat{x}}_{\bar{k}}) \\ {H}_k{P}_k{H}_k^T=(I-K^{\prime })H_k{P}_{\bar{k}}{H}_k^T \end{gathered}$$
其中：$K=\frac{K^{\prime }}{H_k}=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R}$

将上式中的$K$替换后，即可得到更新的三个公式：
$$\begin{gathered} K_k=\frac{P_{\bar{k}}{H}_k^T}{H_k{P}_{\bar{k}}{H}_k^T+R} \\ {\hat{x}}_k={\hat{x}}_{\bar{k}}+K_k(z_k-H_k{\hat{x}}_{\bar{k}}) \\ {P}_k=(I-K_k{H}_k)P_{\bar{k}} \end{gathered}$$
至此，便得到了卡尔曼滤波的结果：${\hat{x}}_k$和$P_k$

PS3：这里需要注意的一点是要将状态方程全部转换到测量状态下才可以进行高斯相乘。否则的话，他们的矩阵都不匹配。

四、参数矩阵

上面的公式中的变量及参数都是矩阵的形式，并且矩阵的行列并不一致。这里举例说明下。

假设状态向量$x$为$x_{n\times 1}$，测量向量$z$为$z_{m\times 1}$。控制变量$u$为$u_{a\times 1}$。
$$\{\begin{array}{l}{A}_{n\times n}\\ B_{n\times a}\\ P_{n\times n}\\ Q_{n\times n}\\ H_{m\times n}\\ K_{n\times m}\\ R_{m\times m}\\ I_{n\times n}\end{array}$$