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

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