以下为纯小白对卡尔曼滤波算法的MATLAB实现进行简单总结，以便以后复习。
一、 背景介绍
假设一小车作匀加速运动，初速度为0，加速度为5米每二次方秒，小车上装有速度传感器，采样频率为10Hz，传感器测量噪声为高斯白噪声，需要充分利用这些信息来估计车辆的速度状态，并验证卡尔曼滤波算法的实验原理与过程。
二、卡尔曼滤波原理
早在近百年前，就有人开始采用状态变量模型研究随机过程，随后为了解决对非平稳、多输入输出随机序列的估计问题，卡尔曼提出了递推最优估计理论。它采用状态空间法描述系统，由状态方程和测量方程组成，即知道前一个状态的估计值和最近一个观测数据，采用递推算法估计当前的状态值。其具有以下特点：
（1） 算法是一个递推过程，且状态空间法采用在时域内设计滤波器的方法，因而适用于多维随机过程的估计；离散型卡尔曼滤波算法适用于计算机处理。
（2） 采用递推算法计算，不用知道所有过去的值，用状态方程描述状态变量的动态变化规律，卡尔曼滤波同样适用于非平稳过程。
如图所示，为卡尔曼滤波算法的实现流程图，其基本思路是若有一组强而合理的假设，给出系统的历史测量值，可以建立最大化这些早前测量值的后验概率的系统状态模型。其基本假设是被建系统是线性的，影响测量的噪声属于符合高斯分布的白噪声。
三、卡尔曼滤波的状态方程和测量方程
整个实现过程包含预测更新和测量更新两大部分。
（1） 预测更新：
预测状态量：
预测误差协方差矩阵：
（2） 测量更新：
最优估计状态量：
计算误差增益：
误差协方差矩阵：
设k时刻小车速度为 ，则系统状态方程为：
测量方程为：
结合卡尔曼滤波算法的预测和测量更新流程，可有：
四、结果分析
根据以上理论分析，完成相应matlab程序（代码见后），并画出小车速度的观测值、真实值、滤波值的对比曲线图如下：
五、代码
(1) kalman.m
1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 %日 期： 2020.3.10 3 %程序功能：使用卡尔曼滤波器估计小车匀加速运动时的速度状态 4 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 clear 6 clc 7 clear all 8 A = [1]; 9 B = [1]; 10 U = 0.5; %A,B,U初始值来自于状态方程 11 H = [1]; %H来自于测量方程 12 step = 500; 13 v = normrnd(0,10,1,step); %均值为0，方差为10噪声 14 X_0 = zeros(1,step); %先验估计初始值 15 X0 = zeros(1,step); %后验估计初始值 16 Z = zeros(1,step); %观测值初始值 17 true = 0.5*(1:step); %小车真实速度 18 temp = true + v(1,:); 19 Z(1,:) = temp; %加入噪声的速度，模拟观测值 20 Q = [10]; %状态方程噪声协方差矩阵 21 R = [500]; %测量方程协方差矩阵 22 P0 = [1]; %初始后验估计协方差 23 P_0 = [1]; 24 for i = 2:step 25 [X_0(i),P_0] = kalmanPredictor(A,B,U,P0,Q,X0(i-1)); 26 [X0(i),P0] = kalmanUpdater(H,R,P_0,Z(i),X_0(i)); 27 end 28 figure; 29 plot(Z,\'g.\'); 30 hold on; 31 plot(true,\'r.\'); 32 hold on; 33 plot(X0,\'b\'); 34 grid on; 35 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
请发表评论