卡尔曼滤波详细推导

qq_34911636

已于 2022-04-24 15:51:28 修改

阅读量6.7k

点赞数 10

分类专栏：滤波器文章标签：概率论算法矩阵

于 2020-08-19 16:06:49 首次发布

本文链接：https://blog.csdn.net/qq_34911636/article/details/108070531

版权

滤波器专栏收录该内容

2 篇文章 1 订阅

订阅专栏

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波器的本质是线性最小均方误差估计，而均方误差是协方差矩阵的迹。卡尔曼滤波有好几种公式推导方法，本文从最小二乘估计的方法推导卡尔曼滤波过程。

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

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

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

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

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

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

1、系统的变量定义

x(k) 为系统的真值

z(k) 为系统的测量值

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

系统的状态转移矩阵

为系统的输入增益矩阵

为系统的量测矩阵

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) 代替得出：

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

= [x(k) - k(k)Hx(k)) -(x(k|k-1) - k(k)Hx(k|k-1)] - k(k)u(k)

= (I - k(k)H)x(k) - (I - k(k)H)x(k|k-1) - k(k)u(k)

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

$e(k)e(k)^{T} = [I - k(k)H][x(k) - x(k|k-1)][x(k) - x(k|k-1)]^{T}[I - k(k)H]^{T}$

$- [I - k(k)H][x(k) - x(k|k-1)]u(k)^{T}k(k)^{T}$

$-k(k)u(k)[x(k)-x(k|k-1)]^{T}[I-Hx(k)]^{T}+k(k)u(k)u(k)^{T}k(k)^{T}$

由于 [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)