【數學建模】基於matlab UKF腳踏車狀態估計【含Matlab原始碼 1111期】

一、簡介

著名學者Julier等提出近似非線性函式的均值和方差遠比近似非線性函式本身更容易，因此提出了基於確定性取樣的UKF演算法。 該演算法的核心思想是：採用UT變換，利用一組Sigma取樣點來描述隨機變數的高斯分佈，然後通過非線性函式的傳遞，再利用加權統計線性迴歸技術來近似非x線性函式的後驗均值和方差。 相比於EKF，UKF的估計精度能夠達到泰勒級數展開的二階精度。

1 UT變換

2 取樣策略 根據Sigma點取樣策略不同，相應的Sigma點以及均值權值和方差權值也不盡相同，因此UT變換的估計精度也會有差異，但總體來說，其估計精度能夠達到泰勒級數展開的二階精度。 為保證隨機變數x經過取樣之後得到的Sigma取樣點仍具有原變數的必要特性，所以取樣點的選取應滿足： 下面介紹兩種經常使用的取樣策略：比例取樣和比例修正對稱取樣

3 UKF演算法流程

二、原始碼

```c %% UKF bicycle test clear all close all

% load params from file load('bicycle_data.mat')

use_laser = 1; use_radar = 1;

stop_for_sigmavis = false;

%% Data Initialization x_pred_all = []; % predicted state history x_est_all = []; % estimated state history with time at row number 6 NIS_radar_all = []; % estimated state history with time at row number 6 NIS_laser_all = []; % estimated state history with time at row number 6

est_pos_error_squared_all = []; laser_pos_error_squared_all = [];

P_est = 0.2*eye(n_x); % initial uncertainty matrix P_est(4,4) = 0.3; % initial uncertainty P_est(5,5) = 0.3; % initial uncertainty

%% process noise

acc_per_sec = 0.3; % acc in m/s^2 per sec yaw_acc_per_sec = 0.3; % 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.03; std_radrd = 0.3;

% 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;

% 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};% get timestamp 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); % yaw_dot: yaw velocity
x = [p1; p2; v; yaw; yaw_dot]; % state vector


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'); % Cholesky factorization.
Xi_aug(:,1) = x_aug;

for i=1:n_aug
    Xi_aug(:,i+1) = x_aug + sqrt(n_aug+kappa) * sP_aug(:,i);
    Xi_aug(:,i+1+n_aug) = x_aug - sqrt(n_aug+kappa) * sP_aug(:,i);
end


% Predict sigma points
Xi_pred = zeros(n_x,2*n_aug+1);
for i=1:(2*n_aug+1)
    p1 = Xi_aug(1,i);
    p2 = Xi_aug(2,i);
    v = Xi_aug(3,i);
    yaw = Xi_aug(4,i);
    yaw_dot = Xi_aug(5,i);

    nu_a = Xi_aug(6,i);
    nu_yaw_dd = Xi_aug(7,i);

    if abs(yaw_dot) > 0.001 %turn around
        p1_p = p1 + v/yaw_dot * ( sin (yaw + yaw_dot*delta_t_sec) - sin(yaw));
        p2_p = p2 + v/yaw_dot * ( cos(yaw) - cos(yaw+yaw_dot*delta_t_sec) );
    else                    %not turn around
        p1_p = p1 + v*delta_t_sec*cos(yaw);
        p2_p = p2 + v*delta_t_sec*sin(yaw);
    end

    v_p = v;
    yaw_p = yaw + yaw_dot*delta_t_sec;
    yaw_dot_p = yaw_dot;

    % add noise
    p1_p = p1_p + 0.5*nu_a*delta_t_sec^2 * cos(yaw);
    p2_p = p2_p + 0.5*nu_a*delta_t_sec^2 * sin(yaw);
    v_p = v_p + nu_a*delta_t_sec;

    yaw_p = yaw_p + 0.5*nu_yaw_dd*delta_t_sec^2;
    yaw_dot_p = yaw_dot_p + nu_yaw_dd*delta_t_sec;

    Xi_pred(1,i) = p1_p;
    Xi_pred(2,i) = p2_p;
    Xi_pred(3,i) = v_p;
    Xi_pred(4,i) = yaw_p;
    Xi_pred(5,i) = yaw_dot_p;
end

% average and covar of sigma points
x_pred = 0;
P_pred = zeros(5,5);

for i=1:2*n_aug+1
    x_pred = x_pred + w(i)* Xi_pred(:,i);
end


for i=1:2*n_aug+1
    P_pred = P_pred + w(i)* (Xi_pred(:,i) - x_pred)*(Xi_pred(:,i) - x_pred)';
end



%% visualize sigma point examples
if stop_for_sigmavis && k == 25
    disp('Stopping for sigma point visualization');

    % 2d example
    P_s = P_est (1:2,1:2);
    x_s = x(1:2);
    Xi_s = zeros(2,5);
    A = chol(P_s,'lower');
    Xi_s(:,1) = x_s;

    for i=1:2
        Xi_s(:,i+1) = x_s + sqrt(3) * A(:,i);
        Xi_s(:,i+1+2) = x_s - sqrt(3) * A(:,i);
    end

    error_ellipse(P_s,x_s,'conf', 0.4, 'style', 'k-');

    Xi_aug_p1 =  squeeze(Xi_s(1,:,:));
    Xi_aug_p2 =  squeeze(Xi_s(2,:,:));

    hold on;
    plot(Xi_aug_p1, Xi_aug_p2, 'or');
    legend('P', 'sigma points')
    axis equal

    xlabel('p_x in m');
    ylabel('p_y in m');

    save('sigma_visualization.mat', 'x_s','P_s','A','Xi_s', 'Xi_aug', 'Xi_pred');

    %return;
end
k=k+1;

```

三、執行結果

四、備註

版本：2014a