卡尔曼滤波详细推导_卡尔曼滤波推导_qq_34911636的博客

相关文章推荐

考研的猕猴桃 · 卡尔曼滤波详细推导_卡尔曼滤波推导_qq_3 ...· 3 年前 ·

考研的猕猴桃 · 卡尔曼滤波-- ...· 3 年前 ·

考研的猕猴桃 · 卡尔曼滤波(Kalman filter) ...· 3 年前 ·

考研的猕猴桃 · 卡尔曼滤波最完整公式推导- 知乎· 3 年前 ·

考研的猕猴桃 · 3、卡尔曼滤波器-------卡尔曼滤波公式 ...· 3 年前 ·

卡尔曼滤波有以下几个特点：

(1)卡尔曼滤波处理的对象时随机信号

(2)被处理信号无有用和干扰之分，滤波的目的是要估计出所有被处理信号

(3)系统的白噪声激励和量测噪声并不是需要滤除的对象，它们的统计特性正式估计过程中需要利用的信息

所以确切的说卡尔曼滤波是属于最优估计理论，与常规滤波器具有完全不同的概念和含义，就实际运用而言卡尔曼滤波实质上

是由一套数字计算机实现的递推算法，每个递推周期中包含对被估计量的时间更新和量测更新过程。

1、系统的变量定义

$x(k)$ 为系统的真值

$z(k)$ 为系统的测量值

$k(k)$ 为卡尔曼滤波增益

$A$ 系统的状态转移矩阵

$B$ 为系统的输入增益矩阵

$H$ 为系统的量测矩阵

$P(k|k)$ 为最优估计协方差矩阵

$P(k|k-1)$ 为预测值协方差矩阵

$x(k|k)$ 为k时刻的最优估计值

$x(k|k-1)$ 为k时刻的预测值，系统的后验概率估计

$x(k-1|k-1)$ 为k-1时刻的最优估计值

$u(k)$ 为系统的测量噪声，其均值 $E(u(k)) = 0$ ，协方差矩阵为R且服从正态分布的测量噪声

$w(k)$ 为系统的过程噪声，其均值 $E(w(k)) = 0$ ，协方差矩阵为Q且服从正态分布的过程噪声

2、系统的状态方程

$x(k) = Ax(k-1) + Bu(k) + w(k)$ ......................................................................(1)

$z(k) = Hx(k) + u(k)$ .......................................................................................(2)

$x(k|k-1) = Ax(k-1|k-1) + Bu(k)$ .........................................................(3)

$x(k|k) = x(k|k-1) + k(k)(z(k) - Hx(k|k-1))$ ........................................(4)

公式(1)为系统状态转移方程，(2)为系统的测量方程，(3)为预测方程（后验概率估计），(4)为最佳估计方程

卡尔曼滤波器最终从上述公式求解出最优估计值 $x(k|k)$ ，在求解最优估计值时首先需要推导出公式(4)的卡尔

曼滤波增益 $k(k)$ ，具体的公式推导见下节

3、卡尔曼滤波推导

遵循误差最小原则，定义误差 $e(k) = x(k) - x(k|k)$ 求误差最小化， $P(k|k)$ 为误差的协方差矩阵

$P(k|k) = E[(x(k) - x(k|k))(x(k)-x(k|k))^{T}]$ ................................................(5)

把误差 $e(k) = x(k) - x(k|k)$ 中的最优估计值 $x(k|k)$ 用公式(4)替代

$e(k) = x(k) - \left \{ x(k|k-1) + k(k)(z(k) - Hx(k|k-1)) \right \}$ .............................(6)

上述误差方程中的 $z(k)$ 用公式(2) $z(k) = Hx(k) + u(k)$ 代替得出：

$= [I - k(k)H][x(k) - x(k|k-1)] - k(k)u(k)$ ................................................(7)

由于 $[x(k) - x(k|k-1)]$ 与 $u(k)$ 无关，并且 $E[u(k)] = 0$

$E[[x(k) - x(k|k-1)][x(k) - x(k|k-1)]^{T}] = P(k|k-1)$ 所以 $P(k|k)$ 协方差矩阵如下：

$P(k|k) = E[e(k)e(k)^{T}]= [I - k(k)H]P(k|k-1)[I - k(k)H]^{T} + k(k)Rk(k)^{T}$ ...........(8)

综上所示我们得到了公式(8)，我们需对公式(8)求 $k(k)$ 的导数，使之 $P(k|k)$ 最小。

我们需将公式(8)展开如下所示：

$P(k|k) = P(k|k-1) - (k(k)HP(k|k-1))^{T} - k(k)HP(k|k-1) \\+ k(k)[HP(k|k-1)H + R]k(k)^{T}$ ....................(9)

卡尔曼滤波器的本质是线性最小均方误差估计，而均方误差是协方差矩阵的迹，所以我们

只需对 $P(k|k)$ 的迹求导 $\frac{\partial tr(P(k|k))}{\partial k(k)}$ 。由于 $tr(A^{T}) = tr(A)$ ，所以 $tr(P(k|k-1))$ 如下：

$tr(P(k|k-1)) = tr(P(k|k-1)) - 2tr[k(k)HP(k|k-1)] \\+ tr[k(k)[HP(k|k-1)H + R]k(k)^{T}]$ .................................(10)

对迹求导我们需要知道以下几个矩阵迹求导公式：

$\frac{\partial tr(AB)}{\partial A} = B^{T}$ 若矩阵B是对称阵则 $\frac{\partial tr(ABA^{T})}{\partial A} = 2AB$ 其中tr表示矩阵的迹。

$\frac{\partial tr(P(k|k))}{\partial k(k)} = - 2P(k|k-1)^{T}H^{T} + 2k(k)[HP(k|k-1)H + R]$ ...............................(11)

令 $\frac{\partial tr(P(k|k))}{\partial k(k)} = 0$ 得出： $k(k) = \frac{P(k|k-1)H^{T}}{HP(k|k-1)H^{T} + R}$ ..........................................(12)

把公式(12)的表达式替换公式(9)最后一项 $k(k)[HP(k|k-1)H + R]k(k)^{T}$ 的 $k(k)$ 得到以下式子：

$= P(k|k-1) - k(k)HP(k|k-1) = (I - k(k)H)P(k|k-1)$ ..................................(13)

当 $k(k)$ 的取值等于公式(12)时，最优估计值的协方差矩阵 $P(k|k)$ 方差最小。但该公式中有一个

未知的量 $P(k|k-1)$ 预测协方差矩阵，接下去我们将就继续推导 $P(k|k-1)$ 的表达式。

定义预测误差 ${e}' = x(k) - x(k|k-1)$ ，预测协方差矩阵 $p(k|k-1)=e^{'}(e^{'})^{T}$

式子 $x(k)$ 用公式(1)替代， $x(k|k-1)$ 用公式(3)替代得出：

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波器的本质是线性最小均方误差估计，而均方误差是协方差矩阵的迹。卡尔曼滤波有好几种公式推导方法，本文从最小二乘估计的方法推导卡尔曼滤波过程。1、系统的变量定义为系统的真值为系统的测量值为卡尔曼滤波增益系统的状态转移矩阵为系统的输入增益矩阵为系统的量测矩阵为最优估计协方假设测一段距离，第一次测z_1=50.1mm，第二次测z_2=50.4mm，第三次测z_3=50.2mm，若想得到准确的值，可以计算这几次测量的平均值, 据此，可以定义估计真实数据等于平均值，可得到如下公式 x^k=1k(x1+x2+⋯xk)\hat{x }_k=\frac{1}{k }(x_1+x_2+⋯x_k )x^k=k1(x1+x2+⋯xk) =1k(x1+x2+ 在 卡尔曼滤波 中，有两种变量。第一种是我们预测的状态变量，是我们从日常的生活经验中总结出来的普遍公式来进行预测的，就像一辆小车在镜面上行驶，速度2m/s,我们通过s = v * t，预测下一秒会前进两米，但实际结果与预测有差异，因为我们的公式没办法将实际的情况全部体现出来（像摩擦力，风速等未在公式中体现）。第二种就是我们实际测量的观察变量，我们用尺子测量小车在一秒内行进的距离。这个

最近重新学习了 卡尔曼滤波 器，为防止忘记了而每次又需要的时候都要找资料学习，又恰逢这次遇到了讲的特别好的老师，所以特别花时间把这一部分整理出来，并希望在整理完知识后再编程实现个相关的案例，以加深对知识的学习与巩固。 3，协方差 4，协方差矩阵

在学习 卡尔曼滤波 的时候看了很多博客讲这方面的知识，感觉都讲得表面的东西，无法了解它五个公式真正代表的过程，这篇博客我想以我的理解讲讲 卡尔曼滤波 。首先我先写出 卡尔曼滤波 的具体过程，首先针对如下状态空间模型（因为 卡尔曼滤波 都是针对状态空间模型进行滤波的）： KF的概率图模型其中 xtx_txt 表示隐状态，yty_tyt 表示量测，黑色的箭头表示状态转移，红色的箭头表示测量，P(xt│xt−1)P(x_t│x_{t-1})P(xt│xt−1) 为状态转移的概率，P(yt│xt)P(y_t│x_t )P(yt│xt)为量测概率。