以下为纯小白对卡尔曼滤波算法的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

	

课程说明
【内容简介】《第二版卡尔曼滤波与应用和MATLAB程序详解视频》共8章45节视频，总学时1716分钟，合28.6小时。
主要内容包括：卡尔曼滤波数学模型及MATLAB程序轻松入门，标准卡尔曼滤波处理线性离散随机系统算法再提升，卡尔曼滤波理论简介与算法主要参数作用，卡尔曼滤波在定位和视频跟踪与估计实际信号等方面的应用，扩展卡尔曼滤波(EKF)处理非线性微分随机系统及其应用3例，无迹卡尔曼滤波(UKF)处理非线性离散随机系统及其应用，交互式多模型(IMM)滤波及其应用与推广问题。全部提供MATLAB代码程序和PPT课件。提供辅导答疑。
讲师介绍
郑一
三级教授，近四十年教学经验
擅长科研及工程技术所用算法和MATLAB程序密切结合；擅长算法+程序+视频的教学设计与录像教学。
-END-
扫码联系客服，加matlab交流群领海量资料！
点击阅读原文，免费试看！