找回密码
 注册
关于网站域名变更的通知
查看: 494|回复: 3
打印 上一主题 下一主题

基于matlab UKF自行车状态估计

[复制链接]

该用户从未签到

跳转到指定楼层
1#
发表于 2021-9-7 15:08 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式

EDA365欢迎您登录!

您需要 登录 才可以下载或查看,没有帐号?注册

x

+ K7 _: Z6 R3 _" {一、简介$ y* ^# L7 g( `( o( B
著名学者Julier等提出近似非线性函数的均值和方差远比近似非线性函数本身更容易,因此提出了基于确定性采样的UKF算法。
2 P! X: [! F- y- X% I3 \该算法的核心思想是:采用UT变换,利用一组Sigma采样点来描述随机变量的高斯分布,然后通过非线性函数的传递,再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。* k" c& e* Y0 @' `5 d2 p9 y
相比于EKF,UKF的估计精度能够达到泰勒级数展开的二阶精度。& ?- X" A9 T, n

) q+ k" l+ S; H# F* L1 UT变换8 ?6 |, ^7 |/ ~( e3 `7 F5 }
. ], A# N* a0 r( x4 W

/ \, o9 z2 P- F+ Q, G3 M" V# Z* g, C

( e$ ^: P2 l; a  b% N. q2 采样策略
7 w, E1 x1 f* G% @
8 e/ i9 S; K0 d) r' G5 L$ S根据Sigma点采样策略不同,相应的Sigma点以及均值权值和方差权值也不尽相同,因此UT变换的估计精度也会有差异,但总体来说,其估计精度能够达到泰勒级数展开的二阶精度。$ m5 F3 w* M) V+ K! M2 U$ I$ ~
为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性,所以采样点的选取应满足:
1 T8 `, L& R4 L, C2 m; N
2 W" W; e: ^" \( J7 B5 i + a8 e) `: v9 ^, U# B9 Z: a

5 Y0 D& q" a3 }+ W1 [4 L# q$ q" a. @* \/ a9 S; Q! W9 S: P: }
下面介绍两种经常使用的采样策略:比例采样和比例修正对称采样" N9 u, I; X* U( `0 m8 M) n

9 y  a" p5 u: a + E" y4 e! L4 I2 B" b! W
& y3 J$ A5 v* k) g3 U

: Y8 d- k. y% I9 J+ U; Y7 Y
; v( s& P. v( a7 @  e* v2 T $ ^1 m/ Q# b0 G( C( w
4 V4 B* m0 ~" E# e  Z
" Q8 w: S6 y3 F
( @+ o* Z$ E7 H# k# [3 Y4 p, e

( H* _2 M; S  c3 a* {  k6 \3 UKF算法流程
2 f3 \. j% K: V1 }
9 r# X* o* p6 J% D 8 O+ t# w* B$ P9 v

: O6 S+ ?+ Y# ]0 q! }6 w " _9 S& |+ c( k& t, I: T
& J. }( Y' h# L* ^- Y
0 R; s- ?/ F4 E
二、源代码
1 I. c/ s  _; o% @
4 c5 a, s+ Q$ C6 k  C7 h
  • %% 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;' d; w, _4 A& p
  s& f  N4 H$ |3 k

. X$ `* A9 F. M  ]2 e三、运行结果! T/ F  U* W$ F& ?- O  \
* j" v# F5 \$ i  E

& M# t* v0 \: M$ i
4 Y- l' e& O: a4 x2 U ! n1 C& ^: A$ e( y' o7 F' o6 A; b  x
0 b7 d) K% f1 s: J
; @# d# @$ s9 C8 P. ?' g6 ~: C6 s

* Y, ^& \- k0 K  @- N# c
( X5 @1 k9 Q' k3 s2 N6 i8 D  C, }) `  B# m

. k2 {4 w$ X/ @9 v6 O# M
  • TA的每日心情

    2019-11-19 15:29
  • 签到天数: 1 天

    [LV.1]初来乍到

    2#
    发表于 2021-9-7 16:17 | 只看该作者
    近似非线性函数的均值和方差远比近似非线性函数本身更容易,因此提出了基于确定性采样的UKF算法

    该用户从未签到

    3#
    发表于 2021-9-7 16:32 | 只看该作者
    相比于EKF,UKF的估计精度能够达到泰勒级数展开的二阶精度

    该用户从未签到

    4#
    发表于 2021-9-7 16:32 | 只看该作者
    采用UT变换,利用一组Sigma采样点来描述随机变量的高斯分布,然后通过非线性函数的传递,再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差
    您需要登录后才可以回帖 登录 | 注册

    本版积分规则

    关闭

    推荐内容上一条 /1 下一条

    EDA365公众号

    关于我们|手机版|EDA365电子论坛网 ( 粤ICP备18020198号-1 )

    GMT+8, 2025-10-5 06:37 , Processed in 0.125000 second(s), 26 queries , Gzip On.

    深圳市墨知创新科技有限公司

    地址:深圳市南山区科技生态园2栋A座805 电话:19926409050

    快速回复 返回顶部 返回列表