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

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

一、 背景介绍

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

二、卡尔曼滤波原理

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

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

整个实现过程包含预测更新和测量更新两大部分。
(1) 预测更新:
预测状态量:
在这里插入图片描述
预测误差协方差矩阵:
在这里插入图片描述
(2) 测量更新:
最优估计状态量:
在这里插入图片描述
计算误差增益:
在这里插入图片描述
误差协方差矩阵:
在这里插入图片描述
设k时刻小车速度为 ,则系统状态方程为:
在这里插入图片描述
测量方程为:
在这里插入图片描述
结合卡尔曼滤波算法的预测和测量更新流程,可有:
在这里插入图片描述

四、结果分析

根据以上理论分析,完成相应matlab程序(代码见后),并画出小车速度的观测值、真实值、滤波值的对比曲线图如下:
基于MATLAB的卡尔曼滤波算法实现_第2张图片

五、代码

(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

你可能感兴趣的:(学习总结,matlab,人工智能,卡尔曼滤波算法)