卡尔曼滤波的原理、理解与仿真

aimzzyTSENG

Be Positive Be Patient

卡尔曼滤波器是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。而且由于观测包含系统的噪声和干扰的影响，所以最优估计也可看做是滤波过程。

1 卡尔曼滤波的原理与理解

1.1 预测

假设有一辆小车，其在t时刻的位置为 P_t (假设其在一维直线上运动，则位置可以用数轴上的点表示)，速度为 v_t 。

因此在t时刻小车的状态可用向量表示为 x_t=\left[ p_t ,v_t\right]^T 。

但是我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。如汽车司机可能会操纵油门，让汽车加速。

假设由于油门的设置或控制命令，我们知道了期望的加速度为 u_t (加速度理解为外部的控制量)，则可由运动学公式从t-1时刻推出其在t时刻的速度与位置如下:

进一步的可以将其写成向量形式:

即:

令

则通过变量代换可以得到 状态转移公式 :

其中：

矩阵 F_t 为状态转移矩阵，表示如何从上一状态来推测当前时刻的状态；

B_t 为控制矩阵，表示控制量 u_t 如何作用于当前矩阵；

\bar{x}_t 有顶帽子，表示只是估计值，并不是最优的。

到这里，我们已经得到了t时刻的状态预测，但这里只有状态预测的均值（即 \bar{x}_t ）。我们是基于高斯分布来建立状态变量的，因此还需要协方差。（即一个多维高斯分布由均值和协方差决定）

假设我们用 P 来表示状态协方差，即

那么加入状态转换矩阵F后有

协方差矩阵的性质有:cov(Ax,Bx)=A*cov(x,x)*B'

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。但是，如果存在外部未知的干扰呢？

例如，轮子可能会打滑，或者路面上的坡会让车减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。

在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型。我们将这些没有被跟踪的干扰当作协方差为 Q_t 的噪声来处理。

状态协方差的预测为：

从而，我们得到了状态均值和协方差的预测，即

现在我们得到上面的两个公式，运用这两个公式能够对现在状态进行预测。但是预测结果不可能完全正确嘛，肯定有误差。而恰好，我们可能会有多个传感器(如测距雷达)来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息，因此我们可以用测量来修正(更新)预测。

1.2 更新

传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用H来表示传感器的数据。

我们可以计算出传感器读数的分布，用之前的表示方法如下式所示

其中H为观测矩阵，v为观测噪声。

卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。

我们将这种不确定性（例如：传感器噪声）用协方差 R_t 表示，该分布的均值就是我们读取到的传感器数据，称之为 z_t 。

现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。我们必须在'预测值'和'传感器测量值'之间找到最优解。我们只需将这两个高斯分布(前一状态的预测以及传感器的测量)相乘就可以了。

以两个一维高斯分布为例，其融合高斯分布为：

其中的K称为卡尔曼增益。

带入状态预测和传感器测量协方差可得到:

K的作用：

(1)K权衡预测协方差P和观察协方差矩阵R那个更加重要。相信预测，则残差的权重小；相信观察，则残差权重大（由 K 的表达式可推出这个结论）。

(2)将残差的表现形式从观察域转换到状态域（残差与一个标量，通过K转换为向量），由状态 X的更新公式可得到该结论

利用卡尔曼增益，我们可以进行更新操作，即在预测的基础上，把测量也考虑进去，可以得到：

综上，我们就得到了卡尔曼滤波的全部方程，重述如下：

2 示例仿真

（1）假设:

传感器从t=0时刻开始，每秒采集一个位置数据，共采集到100个数据。且传感器的测量伴随着均值为0方差为1的高斯噪声 Z=(1:2:200)+randn(1,100);

传感器提供的观测矩阵为 H=[1,0]

传感器的观测噪声协方差矩阵为 R=1

初始状态为 X=[0;0] 即 [位置;速度]=[0;0]

状态协方差矩阵为 P=[1 0;0 1]

状态转移矩阵为 F=[1 delta(t);0 1]=[1 1;0 1]

外部干扰用状态转移协方差矩阵为 Q=[0.0001, 0 ; 0, 0.0001]

（2）问题:

这100内汽车的速度和位置估计。

（3）求解释路：

利用卡尔曼滤波综合运动学方程计算与传感器测量。

（4）计算结果:

其中横坐标表示位置，纵坐标表示速度。

（5）MATLAB代码:

%卡尔曼滤波(小车[速度,位置]例子)观测数据多
clc,clear,close all
%% 传感器观测
%--------------------------------------------------------------------------
%Z = H * X + v
%X为t-1时刻实际状态
%Z为t-1时刻实际观测数据
%H为测量系统的参数,即观察矩阵
%v为观测噪声,其协方差矩阵为R
%--------------------------------------------------------------------------
Z=(1:2:200); %理想观测值 (汽车的位置,也就是我们要修改的量),设定变化是1:2:200，则速度就是2
noise=randn(1,100); %在理想观测值上叠加方差为1的高斯噪声
Z=Z+noise;%模拟的实际观测值
H=[1,0];%传感器提供的观测矩阵
R=1;%传感器的观测噪声协方差矩阵
%% 初始状态(均值)和状态协方差
%基于高斯分布建立状态变量需要均值和协方差(即X和P)
X=[0;0]; %初始状态 X=[位置;速度]
P=[1 0;0 1]; %状态协方差矩阵
%% 状态转移矩阵(表示如何由上一状态推测当前状态),它同时作用与X和P来预测下一时刻的X和P
F=[1 1;0 1]; %在速度例题中为[1 delta(t);0 1]
%% 外部干扰用状态转移协方差矩阵Q表示
Q=[0.0001,0;0 , 0.0001];
%% 卡尔曼滤波
figure;
hold on;
for i = 1:length(Z) %迭代次数
%% 预测
X_ = F*X;%基于上一状态预测当前状态  X_为t时刻状态预测(这里没有控制)
P_ = F*P*F'+Q;%更新协方差  Q系统过程的协方差
%% 计算卡尔曼增益
K = P_*H'/(H*P_*H'+R);
%% 更新
X = X_+K*(Z(i)-H*X_);% 得到当前状态的最优化估算值  增益乘以残差
P = (eye(2)-K*H)*P_;%更新K状态的协方差
%% 绘图
set(0,'defaultfigurecolor','w')