?? nav_data_s1017.m
字號(hào):
%%%%%%%%%%%% End of inserting code %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ve=pve;
vn=pvn;
vu=0;
v0=[ve;vn;vu];
for i=1:maxcount
% dv_1(i)=(ae+bad_a(1))*Ts; % 采樣時(shí)間內(nèi)的速度增量
% dv_2(i)=(an+bad_a(2))*Ts;
% dv_3(i)=(au+bad_a(3)+g)*Ts;
%
% we=0; % 注意緯度是隨時(shí)間變化的，所以，此處的地理坐標(biāo)系內(nèi)的角速度要迭代計(jì)算
% wn=o_miga*cos(fai0);
% wu=o_miga*sin(fai0);
%
% %h0=0;
%
% w_1(i)=we-vn/(R+h0);%+8*pi/180*2*pi/5.0*cos(2.0/5.0*pi*i*Ts); % 角速度
% w_2(i)=wn+ve/(R+h0);%+8*pi/180*2*pi/5.0*cos(2.0/5.0*pi*i*Ts);
% w_3(i)=wu+ve*tan(fai0)/(R+h0);%+8*pi/180*2*pi/5.0*cos(2.0/5.0*pi*i*Ts);% 搖擺周期為5秒，幅度為8度。
% %end
%
% %figure;
% %plot(w3);
%
% %將上面的數(shù)據(jù)轉(zhuǎn)換到載體坐標(biāo)系之中的數(shù)據(jù)為：8*pi/180*2*pi/5.0*cos(2.0/5.0*pi*i*Ts)
% %由于有限角轉(zhuǎn)動(dòng)不為矢量故而要在將旋轉(zhuǎn)角速度投影后再進(jìn)行計(jì)算載體坐標(biāo)系中的角增量
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 開始循環(huán)計(jì)算計(jì)算
%
%
% temp_dv1=dv_1(i);
% temp_dv2=dv_2(i);
% temp_dv3=dv_3(i);
%
% %首先將速度增量和角速度投影到載體坐標(biāo)系之中；
% %考慮到加速度和速度都為矢量，所以積分運(yùn)算和投影運(yùn)算可以不用考慮次序，但是角度不是矢量，所以必須投影完角速度后再進(jìn)行積分運(yùn)算；
%
% temp_C_l_b=C_l_b;
% C_l_b=m_C_l_b;
%
% dv1(i)=C_l_b(1,1)*temp_dv1+C_l_b(1,2)*temp_dv2+C_l_b(1,3)*temp_dv3 ;%+0.001*g*Ts+0.0005*g*rand(1,1)*Ts; % 載體坐標(biāo)系中采樣時(shí)間內(nèi)的的速度增量 ////－－－原始數(shù)據(jù)
% dv2(i)=C_l_b(2,1)*temp_dv1+C_l_b(2,2)*temp_dv2+C_l_b(2,3)*temp_dv3 ;%+0.001*g*Ts+0.0005*g*rand(1,1)*Ts;
% dv3(i)=C_l_b(3,1)*temp_dv1+C_l_b(3,2)*temp_dv2+C_l_b(3,3)*temp_dv3 ;%+0.001*g*Ts+0.0005*g*rand(1,1)*Ts;
%
dv1(i)=deltaVe(i);
dv2(i)=deltaVn(i);
dv3(i)=deltaVu(i);
%%%
% temp_w1=w_1(i);
% temp_w2=w_2(i);
% temp_w3=w_3(i);
%
% w1(i)=C_l_b(1,1)*temp_w1+C_l_b(1,2)*temp_w2+C_l_b(1,3)*temp_w3;% +0.1*pi/180/3600+0.05*pi/180/3600*rand(1,1); % 載體坐標(biāo)系內(nèi)的角速度
% w2(i)=C_l_b(2,1)*temp_w1+C_l_b(2,2)*temp_w2+C_l_b(2,3)*temp_w3;% +0.1*pi/180/3600+0.05*pi/180/3600*rand(1,1);
% w3(i)=C_l_b(3,1)*temp_w1+C_l_b(3,2)*temp_w2+C_l_b(3,3)*temp_w3;% +0.1*pi/180/3600+0.05*pi/180/3600*rand(1,1);
%
% dseit1(i)=w1(i)*Ts; % 載體坐標(biāo)系內(nèi)采樣時(shí)間內(nèi)的角度增量 //////－－－－原始數(shù)據(jù)
% dseit2(i)=w2(i)*Ts;
% dseit3(i)=w3(i)*Ts;
dseit1(i)=deltaSe(i);
dseit2(i)=deltaSn(i);
dseit3(i)=deltaSu(i);
% C_l_b=temp_C_l_b;
%地理坐標(biāo)系相對(duì)于慣性坐標(biāo)系的旋轉(zhuǎn)角速度在地理坐標(biāo)系的投影：
w_i_l_l=[-vn/(R+h0); ve/(R+h0)+o_miga*cos(fai0);ve*tan(fai0)/(R+h0)+o_miga*sin(fai0)];
w_i_l_b=C_l_b*w_i_l_l;
%載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的旋轉(zhuǎn)角增量在載體坐標(biāo)系的投影；
dseitl_b_b=[dseit1(i);dseit2(i);dseit3(i)]-w_i_l_b*Ts;
temp_we(i)=dseitl_b_b(1)/Ts;
temp_wn(i)=dseitl_b_b(2)/Ts;
temp_wu(i)=dseitl_b_b(3)/Ts;
%四元數(shù)更新計(jì)算
seit=sqrt((dseitl_b_b(1))^2+(dseitl_b_b(2))^2+(dseitl_b_b(3))^2);
big_S=[ 0 , dseitl_b_b(3) , -dseitl_b_b(2) , dseitl_b_b(1);
-dseitl_b_b(3) , 0 , dseitl_b_b(1) , dseitl_b_b(2);
dseitl_b_b(2) , -dseitl_b_b(1) , 0 , dseitl_b_b(3);
dseitl_b_b(1) , dseitl_b_b(2) , dseitl_b_b(3) , 0 ];
%s=1.0/abs(seit)*sin(abs(seit)/2.0); %%注意此處直接采用了正弦公式來計(jì)算s的值， 當(dāng)seit的值為0時(shí)應(yīng)該注意考慮；
s=0.5-1.0/48*seit^2+1.0/3840*seit^4;
%c=cos(abs(seit)/2.0)-1;
c=-1.0/8*seit^2+1.0/384*seit^4;
q=q0+(big_S*s+I4*c)*q0;
%將四元數(shù)歸一化
q=q/(sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2));
%捷聯(lián)矩陣的計(jì)算
temp_q=q;
q=[temp_q(4);temp_q(1);temp_q(2);temp_q(3)];
C_b_l=[q(1)^2+q(2)^2-q(3)^2-q(4)^2 , 2.0*(q(2)*q(3)-q(1)*q(4)) , 2.0*(q(2)*q(4)+q(1)*q(3)) ;
2.0*(q(2)*q(3)+q(1)*q(4)) , q(1)^2-q(2)^2+q(3)^2-q(4)^2 , 2.0*(q(3)*q(4)-q(1)*q(2)) ;
2.0*(q(2)*q(4)-q(1)*q(3)) , 2.0*(q(3)*q(4)+q(1)*q(2)) , q(1)^2-q(2)^2-q(3)^2+q(4)^2 ];
q=temp_q;
%有害加速度的計(jì)算；
bad=[ 0, -(2.0*o_miga+ve/((R+h0)*cos(fai0)))*sin(fai0), (2.0*o_miga+ve/((R+h0)*cos(fai0)))*cos(fai0);
(2.0*o_miga+ve/((R+h0)*cos(fai0)))*sin(fai0), 0, -vn/(R+h0) ;
-(2.0*o_miga+ve/((R+h0)*cos(fai0)))*cos(fai0), vn/(R+h0), 0 ];
bad_a=bad*[ve;vn;vu] ; %有害加速度
%將比力轉(zhuǎn)換到當(dāng)?shù)氐乩碜鴺?biāo)系
Sb=[ 0 , -dseitl_b_b(3) , dseitl_b_b(2) ;
dseitl_b_b(3) , 0 , -dseitl_b_b(1) ;
-dseitl_b_b(2) , dseitl_b_b(1) , 0 ];
dv_l=C_b_l*(I3+0.5*Sb)*[dv1(i); dv2(i); dv3(i)];
%計(jì)算運(yùn)載體的運(yùn)動(dòng)速度：
%速度增量為:
dv_l=dv_l-bad_a*Ts-[0;0;-g]*Ts;
%速度旋轉(zhuǎn)補(bǔ)償項(xiàng)
dv_rot=0.5*Sb*dv_l;
v=v0+0.5*(dv_l0+dv_l)+dv_rot;
vout_e(i)=v0(1);
vout_n(i)=v0(2);
vout_u(i)=v0(3);
%dv_l0=dv_l;
%v0=v;
% 計(jì)算位置坐標(biāo)：
l(i)=l0+0.5*((v0(1)+v(1))/((R+h0)*cos(fai0)))*Ts;
fai(i)=fai0+0.5*((v0(2)+v(2))/(R+h0))*Ts;
h(i)=h0+0.5*(v0(3)+v(3))*Ts;
sout_e(i)=sout_e0+0.5*(v0(1)+v(1))*Ts;
sout_n(i)=sout_n0+0.5*(v0(2)+v(2))*Ts;
%%循環(huán)更新
q0=q;
dv_l0=dv_l;
v0=v;
l0=l(i);
fai0=fai(i);
h0=h(i);
sout_e0=sout_e(i);
sout_n0=sout_n(i);
C_l_b=C_b_l';
dx(i)=-C_b_l(2,3)*180/pi;
dy(i)=C_b_l(1,3)*180/pi;
dz(i)=-C_b_l(1,2)*180/pi;
ve=v(1);
vn=v(2);
vu=v(3);
%計(jì)算姿態(tài)角已經(jīng)換算成為角度值
roll(i)=atan(-C_b_l(3,1)/C_b_l(3,3))*180/pi;
pitch(i)=asin(C_b_l(3,2))*180/pi;
yaw(i)=atan(-C_b_l(1,2)/C_b_l(2,2))*180/pi;
%c_matrix(i)=C_b_l(2,3);
%保存姿態(tài)陣的值2003,10,8
c11(i)=C_b_l(1,1);
c12(i)=C_b_l(1,2);
c13(i)=C_b_l(1,3);
c21(i)=C_b_l(2,1);
c22(i)=C_b_l(2,2);
c23(i)=C_b_l(2,3);
c31(i)=C_b_l(3,1);
c32(i)=C_b_l(3,2);
c33(i)=C_b_l(3,3);
%結(jié)束保存
%%%%保存角速率信息
ratee(i)=dseitl_b_b(1)/Ts;
raten(i)=dseitl_b_b(2)/Ts;
rateu(i)=dseitl_b_b(3)/Ts;
%%%%%
end
figure;
plot(vout_e, 'r');
hold on;
plot(vout_n,'g');
hold on;
plot(vout_u,'b');
%hold on;
%plot(faint,'-');
title('速度分量(m/s)');
figure;
plot(roll,'r');
hold on;
plot(pitch,'g');
hold on;
plot(yaw,'b');
title('姿態(tài)角分量(度)');
% figure;
% plot(l*180/pi,'r');
% hold on;
% plot(fai*180/pi,'g');
% %hold on;
% %plot(h,'b');
% title('經(jīng)緯度信息(度)');
figure;
plot(h,'b');
hold on;
plot(sout_e,'r');
hold on;
plot(sout_n,'g');
title('位置信息(m)');
% figure;
% plot(dx,'r');
% hold on;
% plot(dy,'g');
% hold on;
% plot(dz,'b');
% title('不對(duì)準(zhǔn)角信息');
for i=1:maxcount*Ts*10
vout_e(i)=vout_e(i/Ts*0.1);
vout_n(i)=vout_n(i/Ts*0.1);
roll(i)=roll(i/Ts*0.1);
pitch(i)=pitch(i/Ts*0.1);
yaw(i)=yaw(i/Ts*0.1);
sout_e(i)=sout_e(i/Ts*0.1);
sout_n(i)=sout_n(i/Ts*0.1);
l(i)=l(i/Ts*0.1);
fai(i)=fai(i/Ts*0.1);
ratee(i)=ratee(i/Ts*0.1);
raten(i)=raten(i/Ts*0.1);
rateu(i)=rateu(i/Ts*0.1);
origratee(i)=origratee(i/Ts*0.1);
origraten(i)=origraten(i/Ts*0.1);
origrateu(i)=origrateu(i/Ts*0.1);
fai(i)=fai(i/Ts*0.1);
c11(i)=c11(i/Ts*0.1);
c12(i)=c12(i/Ts*0.1);
c13(i)=c13(i/Ts*0.1);
c21(i)=c21(i/Ts*0.1);
c22(i)=c22(i/Ts*0.1);
c23(i)=c23(i/Ts*0.1);
c31(i)=c31(i/Ts*0.1);
c32(i)=c32(i/Ts*0.1);
c33(i)=c33(i/Ts*0.1);
aout_e(i)=aout_e(i/Ts*0.1);
aout_n(i)=aout_n(i/Ts*0.1);
end
save data_s vout_e vout_n roll pitch yaw sout_e sout_n l fai ratee raten rateu origratee origraten origrateu fai c11 c12 c13 c21 c22 c23 c31 c32 c33 aout_e aout_n aout_u maxcount Ts;
%figure;
%plot(temp_we,'r');
%hold on;
%plot(temp_wn,'g');
%hold on;
%plot(temp_wu,'b');
%title('測(cè)試角度');?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -