【濾波跟蹤】基於自適應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代碼問題可私信交流。

部分理論引用網絡文獻，若有侵權聯繫博主刪除。