卡尔曼matblef仿真代码+数据
立即下载
资源介绍:
卡尔曼matblef仿真代码+数据
clc;
clear;
% 初始化变量
%真实值
position_rho_measure = [];
position_theta_measure = [];
position_velocity_measure = [];
time_measure = [];
position_x_true = [];
position_y_true = [];
speed_x_true = [];
speed_y_true = [];
% x,y方向的测量值(由非线性空间转到线性空间)
position_x_measure = [];
position_y_measure = [];
speed_x_measure = [];
speed_y_measure = [];
% 先验估计值
position_x_prior_est = []; % x方向位置的先验估计值
position_y_prior_est = []; % y方向位置的先验估计值
speed_x_prior_est = []; % x方向速度的先验估计值
speed_y_prior_est = []; % y方向速度的先验估计值
% 估计值和观测值融合后的最优估计值
position_x_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计值x位置值存入到列表中
position_y_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计值y位置值存入到列表中
speed_x_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计x速度值存入到列表中
speed_y_posterior_est = []; % 根据估计值及当前时刻的观测值融合到一体得到的最优估计y速度值存入到列表中
% 打开文件
file = fopen('C:/Users/19516/Desktop/sample-laser-radar-measurement-data-2.txt');
% 读取文件并处理数据
while ~feof(file)
line = fgetl(file); % 读取一行
curLine = strsplit(line, '\t'); % 使用制表符分割字符串
% 取出radar数据
if strcmp(curLine{1}, 'R')
position_rho_measure(end+1) = str2double(curLine{2});
position_theta_measure(end+1) = str2double(curLine{3});
position_velocity_measure(end+1) = str2double(curLine{4});
time_measure(end+1) = str2double(curLine{5});
position_x_true(end+1) = str2double(curLine{6});
position_y_true(end+1) = str2double(curLine{7});
speed_x_true(end+1) = str2double(curLine{8});
speed_y_true(end+1) = str2double(curLine{9});
% 测量值 由非线性空间转到线性空间
% rho = str2double(curLine{2});
% theta = str2double(curLine{3});
% velocity = str2double(curLine{4});
% position_x_measure(end+1) = rho * cos(theta);
% position_y_measure(end+1) = rho * sin(theta);
% speed_x_measure(end+1) = velocity * cos(theta);
% speed_y_measure(end+1) = velocity * sin(theta);
end
end
% 关闭文件
fclose(file);
for i = 1 : 100
position_x_measure(i) = position_rho_measure(i)*cos(position_theta_measure(i));
position_y_measure(i) = sqrt(position_rho_measure(i)^2 - position_x_measure(i)^2);
speed_x_measure(i) = position_velocity_measure(i)*cos(position_theta_measure(i));
speed_y_measure(i) = position_velocity_measure(i)*sin(position_theta_measure(i));
end
% --------------------------- 初始化 -------------------------
% 用第二帧测量数据初始化
X0 = [position_x_measure(2); position_y_measure(2); speed_x_measure(2); speed_y_measure(2)];
last_timestamp_ = time_measure(2);
P = eye(4);
X_posterior = X0;
P_posterior = P;
% 将初始化后的数据依次送入(即从第三帧速度往里送)
for i = 3:length(time_measure)
% ------------------- 下面开始进行预测和更新,来回不断的迭代 -------------------------
% 求前后两帧的时间差,数据包中的时间戳单位为微秒,除以1e6,转换为秒
delta_t = (time_measure(i) - last_timestamp_) / 1e6;
last_timestamp_ = time_measure(i);
% 状态转移矩阵F,上一时刻的状态转移到当前时刻
F = [1 0 delta_t 0;
0 1 0 delta_t;
0 0 1 0;
0 0 0 1];
% 外界输入矩阵B
B = [delta_t^2/2 0;
0 delta_t^2/2;
delta_t 0;
0 delta_t];
% 计算加速度
if i < 5
acceleration_x_posterior_est = 0;
acceleration_y_posterior_est = 0;
else
acceleration_x_posterior_est = (speed_x_posterior_est(i-3) - speed_x_posterior_est(i-4)) / delta_t;
acceleration_y_posterior_est = (speed_y_posterior_est(i-3) - speed_y_posterior_est(i-4)) / delta_t;
end
% 外界状态矩阵U
U = [acceleration_x_posterior_est; acceleration_y_posterior_est];
% ---------------------- 预测 -------------------------
X_prior = F * X_posterior;
% X_prior = F*X_posterior + B*U;% 不使用加速度
position_x_prior_est(i) = X_prior(1);
position_y_prior_est(i) = X_prior(2);
speed_x_prior_est(i) = X_prior(3);
speed_y_prior_est(i) = X_prior(4);
% Q:过程噪声的协方差
Q = [0.0001 0 0 0;
0 0.00009 0 0;
0 0 0.001 0;
0 0 0 0.001];
% Q = [0.0001 0 0 0;
% 0 0.00009 0 0;
% 0 0 0.001 0;
% 0 0 0 0.001];
% 计算状态估计协方差矩阵P
P_prior = F * P_posterior * F' + Q;
% ------------------- 更新 ------------------------
% 测量的协方差矩阵R
R = [0.000009 0 0;
0 0.009 0;
0 0 9];
% 状态观测矩阵H (EKF,radar)
Px = X_prior(1);
Py = X_prior(2);
Vx = X_prior(3);
Vy = X_prior(4);
position_rho_ = sqrt(Px^2 + Py^2);
if position_rho_ < 1e-8
position_rho_ = 1e-8;
end
% 线性化
H = [Px/position_rho_ Py/position_rho_ 0 0;
-Py/(position_rho_^2) Px/(position_rho_^2) 0 0;
Py*(Vx*Py - Vy*Px)/(position_rho_^3) Px*(Vy*Px - Vx*Py)/(position_rho_^3) Px/position_rho_ Py/position_rho_];
% 计算卡尔曼增益
K = P_prior * H' * inv(H * P_prior * H' + R);
% 测量值
Z_measure = [position_rho_measure(i); position_theta_measure(i); position_velocity_measure(i)];
% z_measure = [sqrt(Px^2 + Py^2);(Px/sqrt(Px^2 + Py^2));sqrt(Vx^2 + Vy^2)];
X_posterior = X_prior + K * (Z_measure - H*X_prior);
position_x_posterior_est(i) = X_posterior(1);
position_y_posterior_est(i) = X_posterior(2);
speed_x_posterior_est(i) = X_posterior(3);
speed_y_posterior_est(i) = X_posterior(4);
% 更新状态估计协方差矩阵P
P_posterior = (eye(4) - K * H) * P_prior;
end
% 可视化显示
figure;
subplot(2, 2, 1);
plot(position_x_measure, 'b-', 'DisplayName', '位置x_测量值');
hold on;
plot(position_x_posterior_est, 'r-', 'DisplayName', '位置x_扩展卡尔曼滤波后的值');
title('位置x');
xlabel('k');
legend;
subplot(2, 2, 2);
plot(position_y_measure, 'b-', 'DisplayName', '位置y_测量值');
hold on;
plot(position_y_posterior_est, 'r-', 'DisplayName', '位置y_扩展卡尔曼滤波后的值');
title('位置y');
xlabel('k');
legend;
subplot(2, 2, 3);
plot(speed_x_measure, 'b-', 'DisplayName', '速度x_测量值');
hold on;
plot(speed_x_posterior_est, 'r-', 'DisplayName', '速度x_扩展卡尔曼滤波后的值');
title('速度x');
xlabel('k');
legend;
subplot(2, 2, 4);
plot(speed_y_measure, 'b-', 'DisplayName', '速度y_测量值');
hold on;
plot(speed_y_posterior_est, 'r-', 'DisplayName', '速度y_扩展卡尔曼滤波后的值');
title('速度y');
xlabel('k');
legend;