【滤波跟踪】基于自适应UKF和UKF算法实现运动刚体的位姿估计附matlab代码

1 内容介绍

本文研究了基于单目视觉的运动刚体位姿估计问题,提出了基于自适应无迹卡尔曼滤波算法(Adaptive Unscented Kalman Filter,AUKF)的位姿估计方法.考虑到运动刚体位姿估计系统的量测方程为非线性且过程噪声统计特征未知,通过递推噪声估计器在线估计过程噪声的均值和方差阵,解决了位姿估计系统中过程噪声统计特性未知时估计精度下降的问题.实验结果表明,AUKF算法提高了位姿估计的精度,并实现了过程噪声统计特性的在线估计.

2 仿真代码

clear all;clc;

ag=1;

flag =1;

t=0.05*ag;

TxtData1 = importdata('Mvideo1.txt');

armjoints = importdata('ralPointFile.txt');

TxtData2 = importdata('Mvideo2.txt');

m = size(TxtData2,1);

% kx2 = 839.321428295768 ;ky2 = 840.483960297146 ;u02 = 243.868668455832 ;v02 = 216.650954197449 ;

% kx1 = 809.345902119970; ky1 = 803.055062922696;u01 = 380.962537796614;v01 = 234.830825833781;

% % kx1 = 802.336514588841 ;ky1 = 804.376231832541 ;u01 = 331.447470345934 ;v01 = 244.468762099674 ;

% % kx2 = 798.050806080183 ;ky2 = 797.408432851774 ;u02 = 358.151014009806 ;v02 = 232.751596763967 ;

% kx2 = 832.054901757104; ky2 = 828.444768253781;u02 = 332.664199846859;v02 = 211.936118674671;% kx1 = 880.050806080183 ;ky1 = 880.408432851774 ;u01 = 369.151014009806 ;v01 = 212.751596763967 ;

kx1 = 803.345902119970;ky1 = 803.055062922696;u01 = 380.962537796614;v01 = 233.830825833781;

kx2 = 830.054901757104;ky2 = 821.444768253781;u02 = 332.664199846859;v02 = 211.936118674671;

focalIndex = [kx1 ky1 u01 v01;kx2 ky2 u02 v02]';

RelatObjCoor = [-35,-80,0;

    35,-80,0; 

    35,-10,0; 

    -35,-10,0;

    -20,-65,0;

    20,-65,0; 

    20,-25,0; 

    -20,-25,0];

Init_X2 = [0;0;0;0;0;0;0;0;0;0.00001;0;0;0.000001;0;0;0.000001;0;0];

Init_X1 = [armjoints(1,1);armjoints(1,2);armjoints(1,3);0;0;0;0;0;0;armjoints(1,4)*pi/180;armjoints(1,5)*pi/180;armjoints(1,6)*pi/180;0.000001;0;0;0.000001;0;0];

x1 = Init_X1;

x2 = Init_X2;

P1 = 10*eye(18);P2 = P1;

%Q = diag([0,0,0,0.5,0.5,0.5,0.1,0.1,0.1,0,0,0,0.5,0.5,0.5,0.1,0.1,0.1],0);

% R = 0.06*eye(8);

%R = 10*diag([0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05]);

x_aukf1=Init_X1;

P_aukf1 = 10*eye(18);

qaukf1=zeros(18,1);

%Qaukf1= 0.1*diag([0.00001,0.00001,0.00001,0.2,0.5,0.5,0.1,0.1,0.1,  0.00001,0.00001,0.00001,0.5,0.5,0.5,0.1,0.1,0.1],0);

Qaukf1 = 0.5*diag([0,0,0,0.5,0.5,0.5,0.1,0.1,0.1,0,0,0,0.5,0.5,0.5,0.1,0.1,0.1],0);

%Qaukf1=zeros(18,18);

raukf1=zeros(16,1);

Raukf1= 10*diag([0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05]);

SData_X1 = zeros(fix(m/ag),6);

SData_X2 = zeros(fix(m/ag),6);

aa1=1.5;

aa2=-0.25;

aa3=0.75;

tic;

armjoints(1:m,1) =armjoints(1:m,1)-0.3;

armjoints(1:m,2) =armjoints(1:m,2)-0.1;

armjoints(1:m,3) =armjoints(1:m,3)-0.5;

armjoints(1:m,4) =armjoints(1:m,4)+0.8;

armjoints(1:m,5) =armjoints(1:m,5)-0.8;

armjoints(1:m,6) =armjoints(1:m,6)+0.5;

for i = 1:m/ag

    z1 = TxtData1(i,:)';

    z2 = TxtData2(i,:)';

    real =   armjoints(i,:)';

    z = [z1,z2];    

%    [ x1,P1 ] = NonlinerUKF(z1,x1,P1,focalIndex,t,RelatObjCoor,1);

    [ x_aukf1,P_aukf1,qaukf1,Qaukf1,raukf1,Raukf1,Q0 ] = NonlinerAUKF(z1,x_aukf1,P_aukf1,focalIndex,t,RelatObjCoor,qaukf1,Qaukf1,raukf1,Raukf1,1,i);

   %[ x2,P2 ] = NonlinerUKF(z,x2,P2,focalIndex,t,RelatObjCoor,4);tim2 = toc;

%    SData_X2(i,:) = [x1(1),x1(2),x1(3),x1(10)*180/pi,x1(11)*180/pi,x1(12)*180/pi];

    SData_X1(i,:) = [x_aukf1(1),x_aukf1(2)+aa2,x_aukf1(3)+aa3,x_aukf1(10)*180/pi,x_aukf1(11)*180/pi,x_aukf1(12)*180/pi];

end

toc

a = 1:m/ag;

save SData3 SData_X1;

%save SData5 SData_X2;

%save TrackTrue armjoints;

subplot(3,2,1);

plot(a,SData_X1(:,1),'r');hold on;

subplot(3,2,2);

plot(a,SData_X1(:,2),'r');hold on;

subplot(3,2,3);

plot(a,SData_X1(:,3),'r');hold on;

subplot(3,2,4);

plot(a,SData_X1(:,4),'r');hold on;

subplot(3,2,5);

plot(a,SData_X1(:,5),'r');hold on;

subplot(3,2,6);

plot(a,SData_X1(:,6),'r');hold on;

subplot(3,2,1);

plot(a,SData_X2(:,1),'b');hold on;

subplot(3,2,2);

plot(a,SData_X2(:,2),'b');hold on;

subplot(3,2,3);

plot(a,SData_X2(:,3),'b');hold on;

subplot(3,2,4);

plot(a,SData_X2(:,4),'b');hold on;

subplot(3,2,5);

plot(a,SData_X2(:,5),'b');hold on;

subplot(3,2,6);

plot(a,SData_X2(:,6),'b');hold on;

% 

% 

subplot(3,2,1);

plot(a,armjoints(:,1),'k');hold on;

subplot(3,2,2);

plot(a,armjoints(:,2),'k');hold on;

subplot(3,2,3);

plot(a,armjoints(:,3),'k');hold on;

subplot(3,2,4);

plot(a,armjoints(:,4),'k');hold on;

subplot(3,2,5);

plot(a,armjoints(:,5),'k');hold on;

subplot(3,2,6);

plot(a,armjoints(:,6),'k');hold on;

3 运行结果

4 参考文献

[1]张鋆豪, 杨旭升, 冯远静,等. 基于自适应无迹卡尔曼滤波和单目视觉的运动刚体位姿估计[C]// 中国控制会议. 2018.

[2]陈玉寅. 基于卡尔曼滤波器的运动刚体位姿估计方法研究. 浙江工业大学.​

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

部分理论引用网络文献，若有侵权联系博主删除。