clear all
v=0; %%目标速度
v_sensor=0;%%传感器速度
t=1; %%扫描周期
xradarpositon=0; %%传感器坐标
yradarpositon=0; %%
ppred=zeros(4,4);
Pzz=zeros(2,2);
Pxx=zeros(4,2);
xpred=zeros(4,1);
ypred=zeros(2,1);
sumx=0;
sumy=0;
sumxekf=0;
sumyekf=0; %%%统计的初值
L=4;
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
azimutherror=0.015; %%方位均方误差
rangeerror=100; %%距离均方误差
processnoise=1; %%过程噪声均方差
tao=[t^3/3 t^2/2 0 0;
t^2/2 t 0 0;
0 0 t^3/3 t^2/2;
0 0 t^2/2 t]; %% the input matrix of process
G=[t^2/2 0
t 0
0 t^2/2
0 t ];
a=35*pi/180;
a_v=5/100;
a_sensor=45*pi/180;
x(1)=8000; %%初始位置
y(1)=12000;
for i=1:200
x(i+1)=x(i)+v*cos(a)*t;
y(i+1)=y(i)+v*sin(a)*t;
end
for i=1:200
xradarpositon=0;
yradarpositon=0;
Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1);
Zmeasure(2,i)=sqrt((y(i)-yradarpositon)^2+(x(i)-xradarpositon)^2)+random('Normal',0,rangeerror,1,1);
xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i));%%观测值
yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i));
measureerror=[azimutherror^2 0;0 rangeerror^2];
processerror=tao*processnoise;
vNoise = size(processerror,1);
wNoise = size(measureerror,1);
A=[1 t 0 0;
0 1 0 0;
0 0 1 t;
0 0 0 1];
Anoise=size(A,1);
for j=1:2*L+1
Wm(j)=1/(2*(L+ramda));
Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda);%+1-alpha^2+belta; %%%权值
if i==1
xerror=rangeerror^2*cos(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*sin(Zmeasure(1,i))^2;
yerror=rangeerror^2*sin(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*cos(Zmeasure(1,i))^2;
xyerror=(rangeerror^2-Zmeasure(2,i)^2*azimutherror^2)*sin(Zmeasure(1,i))*cos(Zmeasure(1,i));
P=[xerror xerror/t xyerror xyerror/t;
xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t^2);
xyerror xyerror/t yerror yerror/t;
xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t^2)];
xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i)) 0 Zmeasure(2,i)*sin(Zmeasure(1,i)) 0 ]';
end
cho=(chol(P*(L+ramda)))';%
for j=1:L
xgamaP1(:,j)=xestimate+cho(:,j);
xgamaP2(:,j)=xestimate-cho(:,j);
end
Xsigma=[xestimate xgamaP1 xgamaP2];
F=A;
Xsigmapre=F*Xsigma;
xpred=zeros(Anoise,1);
for j=1:2*L+1
xpred=xpred+Wm(j)*Xsigmapre(:,j);
end
Noise1=Anoise;
ppred=zeros(Noise1,Noise1);
for j=1:2*L+1
ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xsigmapre(:,j)-xpred)';
end
ppred=ppred+processerror;
chor=(chol((L+ramda)*ppred))';
for j=1:L
XaugsigmaP1(:,j)=xpred+chor(:,j);
XaugsigmaP2(:,j)=xpred-chor(:,j);
end
Xaugsigma=[xpred XaugsigmaP1 XaugsigmaP2 ];
for j=1:2*L+1
Ysigmapre(1,j)=atan(Xaugsigma(3,j)/Xaugsigma(1,j)) ;
Ysigmapre(2,j)=sqrt((Xaugsigma(1,j))^2+(Xaugsigma(3,j))^2);
end
ypred=zeros(2,1);
for j=1:2*L+1
ypred=ypred+Wm(j)*Ysigmapre(:,j);
end
Pzz=zeros(2,2);
for j=1:2*L+1
Pzz=Pzz+Wc(j)*(Ysigmapre(:,j)-ypred)*(Ysigmapre(:,j)-ypred)';
end
Pzz=Pzz+measureerror;
Pxy=zeros(Anoise,2);
本文探讨了基于MATLAB的机器人手臂结构设计，详细阐述了其力学原理和运动仿真。通过正、逆运动学算法验证，确保手臂运动的精准度，并通过仿真环境构建和检测，优化了机器人手臂的控制功能。
最低0.47元/天 解锁文章
扫一扫
1006
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
