卡尔曼滤波(5)

作者: zidea | 来源:发表于2020-09-15 22:03 被阅读0次

卡尔曼滤波及其无人驾驶应用
卡尔曼滤波(5)
惯导与里程计组合（2）
图文并茂，卡曼滤波
iOS-卡尔曼滤波算法
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波

图

然后我们目标是 $tr(P_k)$ 最小，所以我们 $tr(P_k)$

$\begin{aligned} P_k = P_k^- - K_kHP_k^- - P_k^-H^TK_k^T + K_kHP_k^-H^TK_k^T + K_kRK_k^T \end{aligned}$

我们目标也就是让 $tr(P_k)$ 最小

这里看一下 $K_kHP_k^-$ 和 $P_k^-H^TK_k^T$ 这两项，

$P_k^-H^TK_k^T = (P_k^-H^T)(K_k^T)$ 然后对其求转置得到
$\begin{aligned} ( (P_k^-H^T)(K_k^T))^T = K_k(P_k^-H^T)^T\\ K_kH(P_k^-)^T \end{aligned}$

因为 $P_k$ 是对角矩阵所以 $P_k^T$ 等于 $P_k$ , 所以可以将 $P_k^-H^TK_k^T$ 看成 $K_kHP_k^-$ 的转置矩阵。

$\begin{aligned} tr(P_k) = tr(P_k^-) - 2tr(K_kHP_k^-) + tr(K_kHP_k^-H^TK_k^T) + tr(K_kRK_k^T) \end{aligned}$

就极值问题可以转换为求导问题所以

$\frac{d tr(P_k)}{d K_k} = 0$

$\frac{tr(AB)}{dA} = B^T$
下面通过例子给大家解释一下，

$A = \begin{bmatrix} a_{11} & a_{12} \\ a_{21} & a_{22} \\ \end{bmatrix} \, B = \begin{bmatrix} b_{11} & b_{12} \\ b_{21} & b_{22} \\ \end{bmatrix}$

$AB= \begin{bmatrix} a_{11}b_{11} + a_{12}b_{21} & a_{11}b_{12} + a_{12}b_{22} \\ a_{21}b_{11} + a_{22}b_{21} & a_{21}b_{12} + a_{22}b_{22} \end{bmatrix}$

$tr(AB) = a_{11}b_{11} + a_{12}b_{21} + a_{21}b_{12} + a_{22}b_{22}$

$\frac{dtr(AB)}{dA} = \begin{bmatrix} \frac{\partial tr(AB)}{\partial a_{11}} & \frac{\partial tr(AB)}{\partial a_{12}} \\ \frac{\partial tr(AB)}{\partial a_{21}} & \frac{\partial tr(AB)}{\partial a_{22}} \\ \end{bmatrix} = \begin{bmatrix} b_{11} & b_{21}\\ b_{12} & b_{22}\\ \end{bmatrix} = B^T$

$\frac{d ABA^T}{d A} = 2AB$

$\frac{d tr(P_k)}{d K_k} = 0 - 2(HP_k^-)^T + 2K_kHP_k^-H^T + 2K_kR = 0$

$P_k^-H^T + K_k(HP_k^-H^T + R) = 0$
现在我们得到了卡尔曼核心公式，通过公式可以计算出卡尔曼增益 $K_k$
$K_k = \frac{P_k^-H^T}{HP_k^-H^T + R}$

R 矩阵是噪声协方差矩阵，那么 R 特别大也就是说明噪声特别大，这时因为分母很大所以 $K_k$ 就趋近 0 ，当 $K_k$ 就趋近 0 $\hat{X}_k$ 就等于先验估计因为测量噪声非常大
当 R 非常小， $K_k = \frac{P_k^-H^T}{HP_k^-H^T}= H^-$ 当 $K_k$ 等于 $H^-$ 时候 $\hat{X}_k$ 就等于测量值 $H^-Z_k$