【目标跟踪】基于UKF实现自行车状态估计含Matlab源码

%% UKF bicycle test clear all close all % load params from file load(’bicycle_data.mat’) stop_for_sigmavis = true; %% Data Initialization x_pred_all = []; % predicted state history x_est_all = []; % estimated state history with time at row number 6 P_est = 0.2*eye(n_x); % initial uncertainty P_est(4,4) = 0.3; % initial uncertainty P_est(5,5) = 0.3; % initial uncertainty %% process noise acc_per_sec = 0.2; % acc in m/s^2 per sec yaw_acc_per_sec = 0.2; % yaw acc in rad/s^2 per sec Z_l_read = []; std_las1 = 0.15; std_las2 = 0.15; std_radr = 0.3; std_radphi = 0.0175; std_radrd = 0.1; % UKF params n_aug = 7; kappa = 3-n_aug; w = zeros(2*n_aug+1,1); w(1) = kappa/(kappa+n_aug); for i=2:(2*n_aug+1) w(i) = 0.5/(n_aug+kappa); end %% UKF filter recursion %x_est_all( : ,1) = GT(:,1); Xi_pred_all = []; Xi_aug_all = []; x_est = [0.1 0.1 0.1 0.1 0.01]; last_time = 0; use_laser = 1; use_radar = 0; % load measurement data from file fid = fopen(’obj_pose-laser-radar-synthetic-ukf-input.txt’); %% State Initialization tline = fgets(fid); % read first line % find first laser measurement while tline(1) ~= ’L’ % laser measurement tline = fgets(fid); % go to next line end line_vector = textscan(tline,’%s %f %f %f %f %f %f %f %f %f’); last_time = line_vector{4}; x_est(1) = line_vector{2}; % initialize position p_x x_est(2) = line_vector{3}; % initialize position p_y tline = fgets(fid); % go to next line % counter k = 1; while ischar(tline) % go through lines of data file % find time of measurement if tline(1) == ’L’ % laser measurement if use_laser == false tline = fgets(fid); % skip this line and go to next line continue; else % read laser meas time line_vector = textscan(tline,’%s %f %f %f %f %f %f %f %f %f’); meas_time = line_vector{1,4}; end elseif tline(1) == ’R’ % radar measurement if use_radar == false tline = fgets(fid); % skip this line and go to next line continue; else % read radar meas time line_vector = textscan(tline,’%s %f %f %f %f %f %f %f %f %f %f’); meas_time = line_vector{5}; end else % neither laser nor radar disp(’Error : not laser nor radar’) return; end delta_t_sec = ( meas_time - last_time ) / 1e6; % us to sec last_time = meas_time; %% Prediction part p1 = x_est(1); p2 = x_est(2); v = x_est(3); yaw = x_est(4); yaw_dot = x_est(5); x = [p1; p2; v; yaw; yaw_dot]; std_a = acc_per_sec; % process noise long. acceleration std_ydd = yaw_acc_per_sec; % process noise yaw acceleration if std_a == 0; std_a = 0.0001; end if std_ydd == 0; std_ydd = 0.0001; end % Create sigma points x_aug = [x ; 0 ; 0]; P_aug = [P_est zeros(n_x,2) ; zeros(2,n_x) [std_a^2 0 ; 0 std_ydd^2 ]]; %P_aug = nearestSPD(P_aug); Xi_aug = zeros(n_aug,2*n_aug+1); sP_aug = chol(P_aug,’lower’); Xi_aug( : ,1) = x_aug; for i=1:n_aug Xi_aug( : ,i+1) = x_aug + sqrt(n_aug+kappa) * sP_aug(:,i); % figure(3) % hold on; % plot(GT(1,k), GT(2,k), ’-og’); % plot(x_est(1,:), x_est(2,:), ’-or’); % plot(Z_l(1,k), Z_l(2,k), ’-xb’); % axis equal % legend(’GT’, ’est’, ’Lasermeas’) % k tline = fgets(fid); % read the next line of the data file end fclose(fid); Xi_pred_p1 = squeeze(Xi_pred_all(1,:,:)); Xi_pred_p2 = squeeze(Xi_pred_all(2,:,:)); figure(2) hold on; plot(GT(1, : ), GT(2,:), ’-og’); plot(x_est_all(1, : ), x_est_all(2,:), ’-or’); plot(x_pred_all(1, : ), x_pred_all(2,:), ’.b’); plot(Xi_pred_p1, Xi_pred_p2, ’xb’); legend(’GT’, ’est’, ’pred’, ’Xi pred’) figure(3) hold on; plot(GT(1, : ), GT(2,:), ’-og’); plot(x_est_all(1, : ), x_est_all(2,:), ’-or’); plot(Z_l_read(1, : ), Z_l_read(2,:), ’-xb’); axis equal legend(’GT’, ’est’, ’Lasermeas’) %% figure(1) hold on; plot(GT(8, : ),GT(1,:), ’.-c’); plot(x_est_all(6, : ),x_est_all(1,:), ’-r’); plot(Z_l(3, : ),Z_l(1,:), ’-k’); plot(GT(8, : ),GT(2,:), ’.-b’); plot(x_est_all(6, : ),x_est_all(2,:), ’-r’); plot(Z_l(3, : ),Z_l(2,:), ’-k’); plot(GT(8, : ),GT(3,:), ’.-g’); plot(x_est_all(6, : ),x_est_all(3,:), ’-g’); plot(GT(8, : ),GT(4,:), ’.-r’); plot(x_est_all(6, : ),x_est_all(4,:), ’-r’); plot(GT(8, : ),GT(5,:), ’.-m’); plot(x_est_all(6, : ),x_est_all(5,:), ’-m’); plot(GT(8, : ),[0 diff(GT(3,:))/delta_t_sec], ’-c’); plot(GT(8, : ),[0 diff(GT(5,:))/delta_t_sec], ’.c’); legend(’p1’, ’p1est’,’p1meas’, ’p2’, ’p2est’,’p2meas’, ’v’, ’vest’, ’yaw’, ’yawest’, ’yawrate’, ’yawest’, ’acc’, ’yawacc’)