基于MATLAB的卡尔曼滤波算法实现

以下为纯小白对卡尔曼滤波算法的MATLAB实现进行简单总结，以便以后复习。

一、 背景介绍

假设一小车作匀加速运动，初速度为0，加速度为5米每二次方秒，小车上装有速度传感器，采样频率为10Hz，传感器测量噪声为高斯白噪声，需要充分利用这些信息来估计车辆的速度状态，并验证卡尔曼滤波算法的实验原理与过程。

二、卡尔曼滤波原理

早在近百年前，就有人开始采用状态变量模型研究随机过程，随后为了解决对非平稳、多输入输出随机序列的估计问题，卡尔曼提出了递推最优估计理论。它采用状态空间法描述系统，由状态方程和测量方程组成，即知道前一个状态的估计值和最近一个观测数据，采用递推算法估计当前的状态值。其具有以下特点：
（1） 算法是一个递推过程，且状态空间法采用在时域内设计滤波器的方法，因而适用于多维随机过程的估计；离散型卡尔曼滤波算法适用于计算机处理。
（2） 采用递推算法计算，不用知道所有过去的值，用状态方程描述状态变量的动态变化规律，卡尔曼滤波同样适用于非平稳过程。
如图所示，为卡尔曼滤波算法的实现流程图，其基本思路是若有一组强而合理的假设，给出系统的历史测量值，可以建立最大化这些早前测量值的后验概率的系统状态模型。其基本假设是被建系统是线性的，影响测量的噪声属于符合高斯分布的白噪声。

三、卡尔曼滤波的状态方程和测量方程

整个实现过程包含预测更新和测量更新两大部分。
（1） 预测更新：
预测状态量：

预测误差协方差矩阵：

（2） 测量更新：
最优估计状态量：

计算误差增益：

误差协方差矩阵：

设k时刻小车速度为 ，则系统状态方程为：

测量方程为：

结合卡尔曼滤波算法的预测和测量更新流程，可有：

四、结果分析

根据以上理论分析，完成相应matlab程序（代码见后），并画出小车速度的观测值、真实值、滤波值的对比曲线图如下：

五、代码

(1) kalman.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %日 期： 2020.3.10
 %程序功能：使用卡尔曼滤波器估计小车匀加速运动时的速度状态
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
clc
clear all
A = [1];
B = [1];
U = 0.5;      %A,B,U初始值来自于状态方程
H = [1];       %H来自于测量方程
step = 500;
v = normrnd(0,10,1,step); %均值为0，方差为10噪声
X_0 = zeros(1,step);         %先验估计初始值
X0 = zeros(1,step);           %后验估计初始值
Z = zeros(1,step);             %观测值初始值
true = 0.5*(1:step);           %小车真实速度
temp = true + v(1,:);
Z(1,:) = temp;    %加入噪声的速度，模拟观测值
Q = [10];           %状态方程噪声协方差矩阵
R = [500];         %测量方程协方差矩阵
P0 = [1];           %初始后验估计协方差
P_0 = [1];
for i = 2:step
    [X_0(i),P_0] = kalmanPredictor(A,B,U,P0,Q,X0(i-1));
    [X0(i),P0] = kalmanUpdater(H,R,P_0,Z(i),X_0(i));
end
figure;
plot(Z,'g.');
hold on;
plot(true,'r.');
hold on;
plot(X0,'b');
grid on;
legend('观测值','真实值','kalman滤波');

(2)kalmanPredictor.m

function [Xp,Pp] = kalmanPredictor(A,B,U,P0,Q,X0)
Xp = A*X0 + B*U;
Pp = A*P0*A' + Q;
end

(3)kalmanUpdater.m

function [Xup,Pup] = kalmanUpdater(H,R,Pp,Z,Xp)
I = eye(size(Xp,1));
K = (Pp*H')/(H*Pp*H' + R);
Xup = Xp + K*(Z-H*Xp);
Pup = (I-K*H)*Pp;
end