卡尔曼滤波学习(5)

ylzy

已于 2024-03-20 15:04:33 修改

阅读量1.4k

点赞数 48

分类专栏：卡尔曼滤波文章标签：学习算法机器学习

于 2024-03-14 00:30:25 首次发布

本文链接：https://blog.csdn.net/lglrkx/article/details/136591080

版权

卡尔曼滤波专栏收录该内容

6 篇文章 0 订阅

订阅专栏

本文详细介绍了卡尔曼滤波器中卡尔曼增益的推导过程，通过状态预测方程和观测方程，结合正态分布的噪声模型，优化估计误差，找到最优的卡尔曼增益系数以最小化误差总量。

摘要由CSDN通过智能技术生成

卡尔曼增益推导过程：

前面我们已经得到了状态预测方程：

$X_{k} = A*X_{k-1} + B*U_{k-1} + w_{k-1}$ (1)

观测方程：

$Z_{k} = H * X_{k} + v_{k}$ (2) $X$ ：状态值

$A$ ：状态转移矩阵

$B$ ：控制矩阵

$U$ ：控制变量

$w$ ：过程噪声

$Z$ ：观测值

$v$ ：测量噪声

$w_{k-1}$ 作为过程噪声是不可预测，现在我们假设它是一个理想的符合正态分布的噪声，

则有

$P(w)\sim (0,Q)$

$P(w)$ ： $w$ 的概率分布

0： $w$ 的期望，因为 $w$ 符合正态分布，故数学期望为0

$Q$ ： $w$ 的协方差矩阵

根据概率论协方差矩阵计算方式，我们可以得到 $Q = E[ww^T]$ 。

假设 $X = \begin{bmatrix}x_1 \\ x_2 \end{bmatrix}$ 那么 $w = \begin{bmatrix}w_1 \\ w_2 \end{bmatrix}$ ，则
$Q = E[ww^T] =E[ \begin{bmatrix}w_1 \\ w_2 \end{bmatrix}\begin{bmatrix}w_1 & w_2 \end{bmatrix}]$

$Q = E[\begin{bmatrix}w_1^2 ,w_1w_2 & \\ w_2w_1,w_2^2 & \end{bmatrix}] = \begin{bmatrix}Ew_1^2 ,Ew_1w_2 & \\ Ew_2w_1,Ew_2^2 & \end{bmatrix}$

由 $Var(X) = E(X^2) - E^2(X)$ 和 $w$ 的期望 $E(w)=0$ 可得：

$Var(w) = E(w^2)=\begin{bmatrix}Ew_1^2 ,Ew_1w_2 & \\ Ew_2w_1,Ew_2^2 & \end{bmatrix} = \begin{bmatrix}\sigma_{w1}^2,\sigma_{w1}\sigma_{w2} & \\ \sigma_{w2}\sigma_{w1},\sigma_{w2}^2 & \end{bmatrix}$

同理，我们假设测量噪声 $v_{k}$ 也符合正态分布，则 $P(v)\sim (0,R)$

因为过程噪声和测量噪声是随机噪声，所以是无法进行建模的，也就是公式(1)和(2)只有前半部分是已知可控的，即：

$\hat{X_{k}^-} = A*\hat{X_{k-1}} + B*U_{k-1}$ (3)

$Z_{k} = H * X_{k}$ (4)

根据公式(4)可得

$\hat{X}_kmea = H^-*Z_k$

$\hat{X_{k}^-}$ ：先验估计值

$\hat{X_kmea}$ ：根据测量值计算得到的值

在卡尔曼学习(1)中得到过这个式子 $\hat{x_{k}} = \hat{x_{k-1}} + K_{k} * (z_{k} - \hat{x_{k-1}})$ ，下面我们来详细推导它产生的过程。

设 $\hat{X}_k = \hat{X}_k^- + G(H^-*Z_k - \hat{X}_k^-),G\in [0,1]$ (5)

其中,

$\hat{X}_k$ ：后验值

G：增益值

当 $G = 0$ 时， $\hat{X}_k = \hat{X_k}^-$ ;

当 $G = 1$ 时， $\hat{X}_k = H^-*Z_k$ 。

令 $G = K_kH$ ，则公式(5)可化为：

$\hat{X}_k = \hat{X}_k^- + K_k(Z_k - H\hat{X}_k^-),K\in [0,H^-]$ (6)

当 $K_k = 0$ 时， $\hat{X_k} = \hat{x_k}^-$

当 $K_k = H^-$ 时， $\hat{X_k} = H^-*Z_k$

现在我们目标就是寻找一个合适 $K_k$ 使得 $\hat{X_k}\rightarrow X_k$ ，其中 $X_k$ 是真实值。我们希望当过程噪声 $w$ 较大时，更相信测量结果，而测量噪声 $v$ 较大时，更相信估计结果，所以 $K_k$ 的取值就应该和 $w$ 、 $v$ 相关。

现在我们量化 $\hat{X_k}$ 和 $X_k$ 之间的误差，设 $e_k = \hat{X_k} - X_k$ ，同样 $e_k$ 也是符合正态分布的，则

$P(e_k) \sim (0,p)$

$p = E[ee^T]=\begin{bmatrix}Ee_1^2,Ee_1e_2 & \\ Ee_2e_1,Ee_2^2 & \end{bmatrix} = \begin{bmatrix}\sigma_{e1}^2,\sigma_{e1}\sigma_{e2} & \\ \sigma_{e2}\sigma_{e1},\sigma_{e2}^2 & \end{bmatrix}$

要使 $\hat{X_k}$ 趋近于 $X_k$ ，就是 $\hat{X_k}$ 和 $X_k$ 之间的误差最小，即 $e_k$ 最小，要使 $e_k$ 最小，只需满足 $e_k$ 的协方差矩阵 $p$ 的迹最小，即 $Tr(p) = \sigma_{e1}^2 + \sigma_{e2}^2$ 最小。

那么我们上面的目标就可以转化为，寻找合适的 $K_k$ ，使得 $Tr(p) = \sigma_{e1}^2 + \sigma_{e2}^2$ 最小。方差之和最小，就说明 $e_k$ 的分布更接近于0，那么 $\hat{X_k}$ 和 $X_k$ 就更接近，它们之间的误差就最小。

下面我们先来求解 $e_k$ ，上面给出 $e_k = \hat{X_k} - X_k$ ，根据公式(6)和(4)可得

$e_k = X_k - [X_k^- + K_k(Z_k - HX_k^-))] \\ = X_k - [X_k^- + K_k(HX_k+v_k - HX_k^-))] \\= (X_k - X_k^-) - K_kH(X_k - X_k^-) - K_kv_k \\= (I - K_kH)(X_k-X_k^-)-K_kv_k$ (7)

在公式(7)中的 $(X_k-X_k^-)$ 可以用 $e_k^-$ 表示，即 $e_k$ 的先验值，也称为先验误差。

接下来我们将公式(7)代入 $p$ 中，可以得到如下公式:

$p_k = E[ee^T] = E[[(I-K_kH)e_k^--K_kv_k][(I-K_kH)e_k^--K_kv_k]^T] \\=E[[(I-K_kH)e_k^--K_kv_k][e_k^{-T}(I^T-H^TK_k^T)-v_k^TK_k^T]] \\=E[(I-K_kH)e_k^-e_k^{-T}(I^T-H^TK_k^T)-(I-K_kH)e_k^-v_k^TK_kk^T-K_kv_ke_k^{-T}(I^T-H^TK_k^T)+K_kv_kv_k^TK_k^T] \\=E(I-K_kH)e_k^-e_k^{-T}(I^T-H^TK_k^T)-E(I-K_kH)e_k^-v_k^TK_kk^T-EK_kv_ke_k^{-T}(I^T-H^TK_k^T)+EK_kv_kv_k^TK_k^T$ （8）

注:

$(AB)^T = B^TA^T$

$(A+B)T = A^T+B^T$

当A和B相互独立时 $E(AB) = E(A)E(B)$

因为 $I-K_kH$ 和 $K_k$ 是常量，所以公式(8)可写为：

$p = (I-K_kH)E(e_k^-e_k^{-T})(I^T-H^TK_k^T)-(I-K_kH)E(e_k^-v_k^T)K_kk^T-K_kE(v_ke_k^{-T})(I^T-H^TK_k^T)+K_kE(v_kv_k^T)K_k^T$ （9）

又因为 $e_k^-$ 和 $v_k$ 相互独立，一个是先验误差，一个是测量误差，两个没有相关联系，所以公式（9）可写为：

$p_k = (I-K_kH)E(e_k^-e_k^{-T})(I^T-H^TK_k^T)-(I-K_kH)E(e_k^-)E(v_k^T)K_kk^T-K_kE(v_k)E(e_k^{-T})(I^T-H^TK_k^T)+K_kE(v_kv_k^T)K_k^T$ (10)

$v_k$ 是符合正态分布，即 $E(v_k) = 0$ ，所以由公式(10)可得:

$p_k = (I-K_kH)E(e_k^-e_k^{-T})(I^T-H^TK_k^T)+K_kE(v_kv_k^T)K_k^T$ （11）

因为 $p = E[e_ke_k^T]$ 所以公式(11)中的 $E(e_k^-e_k^{-T})$ 可以写为 $p_k^-$ （先验误差的协方差矩阵），而 $E(v_kv_k^T)$ 就是 $R_k$ (测量误差的协方差矩阵)。

将 $p_k^-$ 和 $R_k$ 代入公式(11)并展开可得：

$p_k = p_k^--K_kHp_k^--p_k^-H^TK_k^T+K_kHp_k^-H^TK_k^T+K_kR_kK_k^T$ （12）

由公式（12）求 $p_k$ 的迹，即:

$Tr(p_k) \\= Tr(p_k^-)-2Tr(K_kHP_k^-)+Tr(K_kHP_k^-H^TK_k^T)+Tr(K_kR_kK_k^T)$

注：

矩阵和矩阵转置的迹相等， $(p_k^-H^TK_k^T)^T = K_k(p_k^-H^T)^T = K_kHp_k^{-T}$

$p_k$ 为堆成矩阵， $p_k=p_k^T$

接下来对 $Tr(p_k)$ 对 $K_k$ 求导，极值点即是 $Tr(p_k)$ 的最小值点: $\frac{\mathrm{d} Tr(p_k)}{\mathrm{d} K_k} =0 - 2(HP_k^-)^T+2K_kHp_k^-H^T+2K_kR_k\\=-P_k^{-T}H^T+K_kHp_k^-H^T+2K_kR_k\\=-p_k^-H^T+K_k(Hp_k^-H^T+R_k) = 0$

即：

$K_k = \frac{p_k^-H^T}{Hp_k^-H^T+R_k}$

当 $R_k$ 特别大时， $K_k\rightarrow 0$

当 $R_k$ 特别小时， $K_k\rightarrow H^-$

至此，卡尔曼增益系数 $K_k$ 推导完毕。

注：

$\frac{\mathrm{d} Tr(AB))}{\mathrm{d} A} = 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}$ 则 $Tr(AB) = a_{11}b_{11}+a_{12}b_{21}+a_{21}b_{21}+a_{22}b_{22}$

$\frac{\mathrm{d} Tr(A))}{\mathrm{d} A} = \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}$

$\frac{\mathrm{d}ABA^T }{\mathrm{d} A} = 2AB$ ，可用上面的方法进行推导。

学习笔记，参考资料【【卡尔曼滤波器】3_卡尔曼增益超详细数学推导～全网最完整】 https://www.bilibili.com/video/BV1hC4y1b7K7/?p=3&share_source=copy_web&vd_source=c29456ffa88bc7559f8ffbe6f8e8f7a5