40行MATLAB代码实现卡尔曼滤波-简单易懂-Toy模板网

这篇具有很好参考价值的文章主要介绍了40行MATLAB代码实现卡尔曼滤波-简单易懂。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

1 前言

最近学习了卡尔曼滤波，体会到了数据融合下进行最优估计的思想。如果你也是小白，可以通过这个例子自己动手感受数据融合。

学习资料参考B站大神DR_CAN博士，连接：【卡尔曼滤波器】直观理解与二维实例

2 案例

基于上述视频中Excel的例子，使用MATLAB编写了一个简单的卡尔曼滤波器，40行代码，简单易懂。

这是一个给匀速行走的人定位的例子，

卡尔曼滤波matlab代码,MATLAB建模,MATLAB教程,MATLAB仿真,matlab,线性代数,矩阵,Powered by 金山文档

假设人作匀速直线运动，根据匀速运动数学模型，就可以得到位置和速度信息（X）。但路上有各种因素，所以这个模型并非理想的，存在一定的误差W。

另外通过卫星（Global Positioning System，GPS）也可以得到人的位置和速度信息（Z），也存在一定的观测误差V。

其中，W和V相互独立，并且都满足均值为0的正态分布。

使用卡尔曼滤波的目的就是：根据上面两种不够精确的方法得到的数据给出一个比较准确的估计。

更新公式如下：

3 代码实现

代码如下：

close all
clear
clc

%%
Q = [1 0;0 1]; %过程误差，Q矩阵
R = [0.1 0;0 0.1]; %观测误差，R矩阵
A = [1 1;0 1]; %A矩阵
H = [1 0;0 1]; %H矩阵
I = [1 0;0 1]; %单位矩阵
epoch = 30;
P = [1 0;0 1];
X = ones(epoch, 2); %生成位置和速度矩阵
X(1,:) =  [0 1];  %初始的实际位置和速度（从位置0开始，假定以匀速1运动）
Xa = ones(epoch, 2);  %用a表示头上的小帽子，后验
Xa(1,:) = [0 1];
Xa_ = ones(epoch, 2);  %先验
Xa_(1,:) = [0 1];  %初始化先验
Z = ones(epoch, 2); %测量的位置和速度
Z(1,:) = [0 1]; %初始测量的位置和速度
for k = 2:epoch
    %更新误差
    w1 = normrnd(0,sqrt(Q(1,1))); %位置过程误差
    w2 = normrnd(0,sqrt(Q(2,2))); %速度过程误差
    W = [w1 w2];
    v1 = normrnd(0,sqrt(R(1,1))); %观测位置过程误差
    v2 = normrnd(0,sqrt(R(2,2))); %观测速度过程误差
    V = [v1 v2];
    %更新实际位置、实际速度
    X(k,:) = (A*X(k-1,:)'+ W')';
    %更新测量位置、测量速度
    Z(k,:) = (H*X(k,:)' + V')';
    %预测
    P_ = A*P*A' + Q;
    Xa_(k,:) = (A*Xa(k-1,:)')'; %先验
    %校正
    K = P_*H'*inv((H*P_)*H' + R);
    P = (I -  K*H)*P_;
    Xa(k,:) = (Xa_(k,:)' + K*(Z(k,:)' - H*Xa_(k,:)'))';
end
%通过以下三个值比较平均绝对误差。mae_Xa 小于mae_Xa_和mae_Z，说明融合有效
mae_Xa = sum(abs(Xa(:,1)-X(:,1)));
mae_Xa_ = sum(abs(Xa_(:,1)-X(:,1)));
mae_Z = sum(abs(Z(:,1)-X(:,1)));
plot(1:epoch, X(:,1), 'r*-', 1:epoch, Z(:,1), '-b*', 1:epoch, Xa_(:,1), '-g*', 1:epoch, Xa(:,1), '-k*','LineWidth',2)

hold on
xlabel step
ylabel 位置
legend('实际位置', '测量位置', '先验估计位置', '后验估计位置')