卡尔曼(kalman)滤波器原理

article/2025/10/5 22:19:40

引言：卡尔曼滤波器适用于线性高斯系统，若为非线性系统，可以使用扩展卡尔曼滤波器。

一、状态估算器

如下图，如果我们需要知道火箭发射时尾部内部的实际温度 $T_{in}$ ，这个温度与火箭的燃料输入 $W_{fuel}$ 有关。但是由于里面温度过高，我们无法直接去用温度传感器去测量，那我们可以在外部用一个温度传感器去测量外部温度 $T_{ext}$ ，利用此测量值来得到内部温度 $T_{in}$ 。

同时，我们也可以推导数学方程，利用输入燃料与外部温度的关系的数学表达式得到一个外部温度 $\widehat{T}_{ext}$ , 也可以利用此温度得到一个内部温度 $\widehat{T}_{in}$ 。但是，不论是 $T_{in}$ 和 $\widehat{T}_{in}$ 都只是一个实际的近似值，这些会受到各种不确定因素的影响。

如果有一个完美的系统，使测量值 $T_{ext}$ 和估算值 $\widehat{T}_{ext}$ 能够互相匹配，那么计算的内部温度也会与真实的内部温度吻合，但是，事实上这样是不行的。这时就需要使用状态估算器来估算内部状态。

我们的目标是使估计的外部温度与测量的外部温度温度吻合，如果这两者相等，那么估算的内部温度也能收敛到真实的内部温度，所以我们要做的是尽量减少估算和测量的外部温度之间的差异，如下图

我们需要使误差e变得尽可能小等于0，使用一个控制器K来对估算数据进行调节（因为测量数据 $T_{ext}$ 是由温度传感器测出的，不能够改变，能人为改变的只有利用数学模型得到的估算温度 $\widehat{T}_{ext}$ ）。这样，我们可以得到下图，

里面阴影部分表示状态观测器，通过与控制器K形成闭环，不断调节输出值 $\widehat{T}_{ext}$ ，来消除测量值 $T_{ext}$ 和估算值 $\widehat{T}_{ext}$ 之间的误差，这样，估算的温度就能够得到真实值。将上面的例子抽象化，得到下面的图

目标变为 $\widehat{x}$ 能够逼近 $x$ ,其中 $\widehat{x}$ 点与 $x$ 点是微分求导符号，将对应的方程相减，得到误差方程，如下图

上图中的 $e_{obs}$ 点是一个微分方程，求解微分方程得到 $e_{obs}$ 如下图

如果A-KC小于0，当t趋向于无穷时， $e_{obs}$ 也会降低趋向于0，这时 $\widehat{x}$ 会收敛到 $x$ 。通过控制控制器的增益K，我们就可以控制 $e_{obs}$ 的衰减率，选择增益K的方法，就是卡尔曼滤波器。

二、最优状态估计

再举一个例子，当汽车行驶在公路上那个，我们需要知道汽车的位置，此时我们就可以用卡尔曼滤波器来进行估计。如下图， $u_k$ 是输入，汽车油门，那就是速度了， $C$ 是观测矩阵，在这里，我们假设简单系统，只有一个位置状态 $x_k$ 为一个标量，则 $C$ 等于1. $y_k$ 得到的结果为观测结果，可以认为是通过GPS得到的位置信息，他是已知的，但是他们都会存在噪声，他们都是随机的。见下图，系统模型变为

其中， $\omega _k$ 和 $v_k$ 是噪声，他们都服从高斯分布，即

$v\sim N(0,R) ,\omega \sim N(0,Q)$

与第一节中类似，我们可以通过一个数学模型来计算汽车的位置，那么我们可以得到下图，

如图，卡尔曼滤波器就是根据观测值和估计值两个信息来确定汽车的最优位置。再解释一下，汽车从位置 $x_{k-1}$ 到位置 $x_k$ 过程中，可以得到两个值，为估计值 $\widehat{x}_{k}$ 和测量值 $y_k$ ，但是他们都存在高斯噪声，所以得到的结果会存在误差，这个误差可以用方差来表示，卡尔曼滤波器就是根据这两个值，得到一个方差更小的的汽车位置估计值。可以看下图，图中灰色的方差更小的估计值就是根据

估计值 $\widehat{x}_{k}$ 和测量值 $y_k$ 来得到的。

三、最优状态估计算法和方程

在这里，直接引入卡尔曼滤波器方程

可以看到他与第一节中的状态观测器方程一样，实际上，卡尔曼滤波器就是一种状态观测器。其中大括号括起来的部分是预估值，即根据前面的值估计得到现在的估计值，重写如下

这就是卡尔曼滤波器的核心方程了。下面给出整个卡尔曼滤波器的方程组

在这里看以看出，卡尔曼滤波器分为两步，预测和更新。

预测部分包括状态预估值和误差协方差P，状态预估值根据k-1的最优状态和当前输入u组成，A是状态转移矩阵，B是输入的一个系数，P是状态预估值的方差，用来反映预估值的不确定性，这种不确定性来自过程噪声和k-1状态的不确定性。

更新部分使用预测部分得到的值，其中K为卡尔曼增益，调整卡尔曼增益，可以使更新后的状态值误差协方差最小。 $\widehat{x}_{k}$ 为更新后的状态值， $P_k$ 为更新后的状态值误差协方差。

循环上面的预测和更新部分，就是完整卡尔曼滤波器了。

注：本文参考b站MATLAB中国，链接http://【【官方中字】什么是卡尔曼滤波器 (Kalman Filters) ？(全7P) MATLAB&Simulink】https://www.bilibili.com/video/BV1V5411V72J?p=6&vd_source=a887b568bfe8c73e3ba0fa465faf2b6e

如有错误，欢迎批评指正！