卡尔曼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;