基于matlab实现的卡尔曼滤波匀加速直线运动仿真

完整程序：

clear
clc
%% 初始化参数
delta_t = 0.1;  %采样时间
T = 8;          %总运行时长
t = 0:delta_t:T;  %时间序列
N = length(t);  %序列的长度  
x0 = 0;  %初始位置
u0 = 0;  %初速度
U = 10;  %控制量、加速度

F = [1 delta_t
    0 1];   %状态转移矩阵
B = [0.5*delta_t^2  
    delta_t];  %控制矩阵
H = [1 0];  %观测矩阵
W = [0;3];  %过程噪声
V = [70];  %量测噪声

%分配空间 
XP = zeros(2,N);%预测值
XP(:,1) = [x0;u0];
XR = zeros(2,N);%真实值
XR(:,1) = [x0;u0];
Z = zeros(1,N);%观测值
Z(1) = [0];

for i=2:N
    XP(:,i) = F*XP(:,i-1)+B*U;  %预测值
    XR(:,i) = F*XR(:,i-1)+B*U+sqrt(W)*randn;  %真实值
    Z(i) = H*XR(:,i)+sqrt(V)*randn;  %观测值
end

%% 卡尔曼滤波%%
%初始化参数
Xk = zeros(2,N);  %最优估计值
Xk(:,1) = [0;0];
P = [1,0;0,1];  %均方误差
I = eye(2);
Q = [0 0;0 0.1];
R = 10;
for i=2:N
    
    %时间更新
    X_pre = F*Xk(:,i-1)+B*U;  %状态预测值
    P_pre = F*P*F'+Q;     %预测均方误差
    
    %量测更新
    Kg = P_pre*H'*inv(H*P_pre*H'+R);  %计算卡尔曼增益
    Xk(:,i) = X_pre+Kg*(Z(:,i)-H*X_pre);  %状态估计值
    P = (I-Kg*H)*P_pre;  %均方误差
end

%% 结果
figure(1)
plot(t,XP(1,:),'K');hold on
grid on
plot(t,XR(1,:),'r');hold on
plot(t,Z(:),'b');
legend('预测值', '真实值','量测值');
title('位置')
xlabel('时间 [sec]')
ylabel('位置 [m]')
hold on
plot(t,Xk(1,:),'g');
legend('预测值', '真实值','量测值','kalman估计值');

figure(2)
x_error = abs(XR-Xk(1,:));
x_error1 = abs(XR-Z(1,:));
plot(t,x_error(1,:),'b'),grid on;hold on 
plot(t,x_error1(1,:),'r');
legend('估计值误差', '量测值误差');
title('位置误差')
xlabel('时间 [sec]')
ylabel('位置均方根误差 [m]')
grid on
hold off;

figure(3)
plot(t,Xk(2,:),'r'),grid on;
title('实际速度 ')
legend('实际速度')
xlabel('时间 [sec]')
ylabel('速度 [m/sec]')
hold off;