📄 slipe.m
字号:
wibb3=winb3+wnbb3;
%%%%%%%%%%%%%%%%%%%%%%
fn(1,1) = a(1) - (2*wien(3)+wenn(3))*v0(2);
fn(2,1) = a(2) + (2*wien(3)+wenn(3))*v0(1);
fn(3,1) = a(3) + (2*wien(1)+wenn(1))*v0(2) - (2*wien(2)+wenn(2))*v0(1) + g1;
%%%%%%%%%%%%%%%%
fbb1=Tnb1*fn;
fbb2=Tnb2*fn;
fbb3=Tnb3*fn;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wib1=Kg*wibb1;%+G_Drift+0.1*G_Drift*randn(1);
wib2=Kg*wibb2;%+G_Drift+0.1*G_Drift*randn(1);
wib3=Kg*wibb3;%+G_Drift+0.1*G_Drift*randn(1);
fb3=Ka*fbb3;%+A_Bias+0.1*A_Bias*randn(1);
%%%%%%%%%%%%%%
q0 = q;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wpbb1=wib1-inv(T)*(wepp+wiep);
wpbb2=wib2-inv(T)*(wepp+wiep);
wpbb3=wib3-inv(T)*(wepp+wiep);
omiga1=[0 -wpbb1(1) -wpbb1(2) -wpbb1(3);
wpbb1(1) 0 wpbb1(3) -wpbb1(2);
wpbb1(2) -wpbb1(3) 0 wpbb1(1);
wpbb1(3) wpbb1(2) -wpbb1(1) 0 ];
omiga2=[0 -wpbb2(1) -wpbb2(2) -wpbb2(3)
wpbb2(1) 0 wpbb2(3) -wpbb2(2)
wpbb2(2) -wpbb2(3) 0 wpbb2(1)
wpbb2(3) wpbb2(2) -wpbb2(1) 0 ];
omiga3=[0 -wpbb3(1) -wpbb3(2) -wpbb3(3)
wpbb3(1) 0 wpbb3(3) -wpbb3(2)
wpbb3(2) -wpbb3(3) 0 wpbb3(1)
wpbb3(3) wpbb3(2) -wpbb3(1) 0 ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
k1 = 1/2*omiga1*q;
q=q0+k1*Hn/2;
k2=1/2*omiga2*q;
q=q0+k2*Hn/2;
k3=1/2*omiga2*q;
q=q0+k3*Hn;
k4=1/2*omiga3*q;
q=q0+(k1+2*k2+2*k3+k4)/6*Hn;
q=q/sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);
T=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3))
2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)-q(1)*q(2))
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2];
fp=T*fb3;
f_vx = fp(1) + (2*wiep(3)+wepp(3))*v(2);
f_vy = fp(2) - (2*wiep(3)+wepp(3))*v(1);
v(1) = v(1) + f_vx*Hn;
v(2) = v(2) + f_vy*Hn;
wepp = [-v(2)/Ryp;
v(1)/Rxp;
v(1)*tan(phi)/Rxp];
phi = phi+v(2)*Hn/Ryp;
lamda= lamda+v(1)*Hn/(Rxp*cos(phi));
wiep = [0; wie*cos(phi); wie*sin(phi)];
g1 = g0+0.051799*sin(phi)*sin(phi);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(k<=time/Hn)
A =[v(2)*tan(phi)/Rxp 2*wiep(3)+wepp(3) 0 -fp(3) fp(2) T(1,1) T(1,2) 0 0 0
-2*(wiep(3)+wepp(3)) 0 fp(3) 0 -fp(1) T(2,1) T(2,2) 0 0 0
0 -1/Ryp 0 wiep(3)+v(1)*tan(phi)/Rxp -(wiep(2)+wepp(2)) 0 0 T(1,1) T(1,2) T(1,3)
1/Rxp 0 -(wiep(3)+wepp(3)) 0 wepp(1) 0 0 T(2,1) T(2,2) T(2,3)
1*tan(phi)/Rxp 0 wiep(2)+wepp(2) v(2)/Ryp 0 0 0 T(3,1) T(3,2) T(3,3)
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0];
B=[ T(1,1) T(1,2) 0 0 0 0 0 0 0 0
T(2,1) T(2,2) 0 0 0 0 0 0 0 0
0 0 T(1,1) T(1,2) T(1,3) 0 0 0 0 0
0 0 T(2,1) T(2,2) T(2,3) 0 0 0 0 0
0 0 T(3,1) T(3,2) T(3,3) 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0];
% B=eye(10);
[F,G] = c2d(A,B,Hn);
FT = F';
GT = G';
HT=H';
z=v-v0;%+0.001*randn(3,1);
Z=[z(1)
z(2)];
Xk = F*X;
Zk = H*Xk;
Pk = F*P*FT + G*Q*GT;
K = Pk*HT*inv(H*Pk*HT+R);
P = (eye(10)-K*H)*Pk;
X = Xk + K*(Z-Zk);
datx1(k) = X(3)/rad_deg*60;
datx2(k) = X(4)/rad_deg*60;
datx3(k) = X(5)/rad_deg*60;
datx6(k) = X(6)/A_Bias(1)*1e-4;
datx7(k) = X(7)/A_Bias(2)*1e-4;
datx8(k) = X(8)/G_Drift(1)*0.01;
datx9(k) = X(9)/G_Drift(2)*0.01;
datx10(k) = X(10)/G_Drift(3)*0.01;
end
if(k==time/Hn)
v=v0;
q(1) = q(1)-q(2)*X(3)/2-q(3)*X(4)/2-q(4)*X(5)/2;
q(2) = q(2)+q(1)*X(3)/2+q(4)*X(4)/2-q(3)*X(5)/2;
q(3) = q(3)-q(4)*X(3)/2+q(1)*X(4)/2+q(2)*X(5)/2;
q(4) = q(4)+q(3)*X(3)/2-q(2)*X(4)/2+q(1)*X(5)/2;
q=q/sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2);
T=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2];
TT=T*inv(Tbn);
dpp=TT(2,3)/rad_deg*60
drr=TT(3,1)/rad_deg*60
dyy=TT(1,2)/rad_deg*60
end
if(k>time/Hn)
A =[0 2*wiep(3)+wepp(3) 0 -fp(3) fp(2)
-(2*wiep(3)+wepp(3)) 0 fp(3) 0 -fp(1)
0 -1/Ryp 0 wiep(3)+v(1)*tan(phi)/Rxp -(wiep(2)+wepp(2))
1/Rxp 0 -(wiep(3)+wepp(3)) 0 wepp(1)
1*tan(phi)/Rxp 0 wiep(2)+wepp(2) v(2)/Ryp 0 ];
B=[ T(1,1) T(1,2) 0 0 0
T(2,1) T(2,2) 0 0 0
0 0 T(1,1) T(1,2) T(1,3)
0 0 T(2,1) T(2,2) T(2,3)
0 0 T(3,1) T(3,2) T(3,3)];
H=[1 0 0 0 0
0 1 0 0 0];
[F,G] = c2d(A,B,Hn);
FT = F';
GT = G';
HT=H';
z=v-v0;
Z=[z(1)
z(2)];
Xk = F*X1;
Zk = H*Xk;
Pk = F*P1*FT + G*Q1*GT;
K = Pk*HT*inv(H*Pk*HT+R1);
P1 = (eye(5)-K*H)*Pk;
X1 = Xk + K*(Z-Zk);
%%%%%%%%%%%%%%固定点平滑新算法%%%%%%%%%%%%
Psf=Pksf*FT;
Ksk=Psf*HT*inv(H*Pk*HT+R);
Xs=Xs+Ksk*(Z-H*Xk);
Pksf=Psf-Ksk*H*Pk;
%%%%%%%%%%%%%%固定点平滑新算法%%%%%%%%%%%%
%%%%%%%%%%%%%%固定点平滑算法%%%%%%%%%%%%
% Psf=Pksf*FT;
% Ksk=Psf*HT*inv(H*Pk*HT+R);
% Xs=Xs+Ksk*(Z-H*Xk);
% Pksf=Psf-Ksk*H*Pk;
%%%%%%%%%%%%%%固定点平滑算法%%%%%%%%%%%%
datas1(k-time/Hn)=Xs(3)/rad_deg*60;
datas2(k-time/Hn)=Xs(4)/rad_deg*60;
datas3(k-time/Hn)=Xs(5)/rad_deg*60;
datx11(k-time/Hn)=X1(3)/rad_deg*60;
datx12(k-time/Hn)=X1(4)/rad_deg*60;
datx13(k-time/Hn)=X1(5)/rad_deg*60;
recode(k-time/Hn,1)={X1};
recode(k-time/Hn,2)={F};
recode(k-time/Hn,3)={Pk};
recode(k-time/Hn,4)={P1};
end
dvx(k)=Z(1);
dvy(k)=Z(2);
end
len=length(recode);
Xk=recode{len,1};
Xkn=Xk;
F=recode{len,2};
Pk=recode{len,3};
P=recode{len,4};
for i=2:len
Xk=recode{len-i+2,1};
F=recode{len-i+2,2};
Pk=recode{len-i+2,3};
P=recode{len-i+2,4};
Kks=P*F'*inv(Pk);
Xkn=Xk+Kks*(Xkn-F*Xk);
dat1(i)=Xkn(3)/rad_deg*60;
dat2(i)=Xkn(4)/rad_deg*60;
dat3(i)=Xkn(5)/rad_deg*60;
end
j=1:length(datx1);
figure(1)
subplot(3,1,1)
plot(j,(datx1(j)-dp))
subplot(3,1,2)
plot(j,(datx2(j)-dr))
subplot(3,1,3)
plot(j,(datx3(j)-dy))
figure(2)
i=1:length(dvx);
subplot(2,1,1)
plot(dvx);
hold on;
subplot(2,1,2)
plot(dvy);
hold off;
i=1:length(datx11);
figure(3)
subplot(3,1,1)
plot(i,(datx11(i)))
subplot(3,1,2)
plot(i,(datx12(i)))
subplot(3,1,3)
plot(i,(datx13(i)))
j=1:length(dat1);
figure(4)
subplot(3,1,1)
plot(j,(dat1(j)))
subplot(3,1,2)
plot(j,(dat2(j)))
subplot(3,1,3)
plot(j,(dat3(j)))
j=1:length(datas1);
figure(5)
subplot(3,1,1)
plot(j,(datas1(j)))
subplot(3,1,2)
plot(j,(datas2(j)))
subplot(3,1,3)
plot(j,(datas3(j)))⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -