【目标跟踪】基于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 6​P_est = 0.2*eye(n_x); % initial uncertaintyP_est(4,4) = 0.3; % initial uncertaintyP_est(5,5) = 0.3; % initial uncertainty​%% process noise​acc_per_sec = 0.2; % acc in m/s^2 per secyaw_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 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 lineend    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_xx_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 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代码问题可私信交流。