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

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

1 简介

UKF 算法是广泛应用的非线性滤波方法之一, 在加性噪声条件下, 根据是否状态扩展和是否重采样有四种实现方式. 从算法精度、适应性和计算效率等方面进行了理论分析和仿真计算, 证明适当选择滤波器参数, 常用采样策略下, 状态扩展与非扩展的 UT 变换结果相同, 但后者的计算效率较高; 加性测量噪声条件下, 扩展与非扩展 UKF 可获得相同的滤波结果; 加性过程噪声条件下, 扩展与非扩展 UKF 仅能获得相同的状态预测结果; 重采样不总是构建滤波器的必要环节, 但理论分析和仿真计算发现了重采样对滤波器增益的自适应调节能力, 指出其在状态偏差或未知机动模式较大时对改善滤波器收敛性和精度有重要贡献.

2 部分代码

%% UKF bicycle testclear allclose all% load params from fileload('bicycle_data.mat') stop_for_sigmavis = true;%% Data Initializationx_pred_all = []; % predicted state historyx_est_all = []; % estimated state history with time at row number 6P_est = 0.2*eye(n_x); % initial uncertaintyP_est(4,4) = 0.3; % initial uncertaintyP_est(5,5) = 0.3; % initial uncertainty%% process noiseacc_per_sec = 0.2; % acc in m/s^2 per secyaw_acc_per_sec = 0.2; % yaw acc in rad/s^2 per secZ_l_read = [];std_las1 = 0.15;std_las2 = 0.15;std_radr = 0.3;std_radphi = 0.0175;std_radrd = 0.1;% UKF paramsn_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 filefid = fopen('obj_pose-laser-radar-synthetic-ukf-input.txt');%% State Initializationtline = fgets(fid); % read first line% find first laser measurementwhile tline(1) ~= 'L' % laser measurement tline = fgets(fid); % go to next lineendline_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_xx_est(2) = line_vector{3}; % initialize position p_ytline = 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 fileendfclose(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 equallegend('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')

3 仿真结果

4 参考文献

[1]杨旭升, 张文安, 俞立. 基于UKF算法的目标跟踪系统设计及实现[J].  2013.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。