📄 nav_data_s1017.m
字号:
%%%%%%%%%%%% 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; % 采样时间内的速度增量
% dv_2(i)=(an+bad_a(2))*Ts;
% dv_3(i)=(au+bad_a(3)+g)*Ts;
%
% we=0; % 注意纬度是随时间变化的，所以，此处的地理坐标系内的角速度要迭代计算
% 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);
%
% %将上面的数据转换到载体坐标系之中的数据为：8*pi/180*2*pi/5.0*cos(2.0/5.0*pi*i*Ts)
% %由于有限角转动不为矢量故而要在将旋转角速度投影后再进行计算载体坐标系中的角增量
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 开始循环计算计算
%
%
% temp_dv1=dv_1(i);
% temp_dv2=dv_2(i);
% temp_dv3=dv_3(i);
%
% %首先将速度增量和角速度投影到载体坐标系之中；
% %考虑到加速度和速度都为矢量，所以积分运算和投影运算可以不用考虑次序，但是角度不是矢量，所以必须投影完角速度后再进行积分运算；
%
% 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; % 载体坐标系中采样时间内的的速度增量 ////－－－原始数据
% 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); % 载体坐标系内的角速度
% 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; % 载体坐标系内采样时间内的角度增量 //////－－－－原始数据
% 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;
%地理坐标系相对于惯性坐标系的旋转角速度在地理坐标系的投影：
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;
%载体坐标系相对于地理坐标系的旋转角增量在载体坐标系的投影；
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;
%四元数更新计算
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); %%注意此处直接采用了正弦公式来计算s的值， 当seit的值为0时应该注意考虑；
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;
%将四元数归一化
q=q/(sqrt(q(1)^2+q(2)^2+q(3)^2+q(4)^2));
%捷联矩阵的计算
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;
%有害加速度的计算；
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] ; %有害加速度
%将比力转换到当地地理坐标系
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)];
%计算运载体的运动速度：
%速度增量为:
dv_l=dv_l-bad_a*Ts-[0;0;-g]*Ts;
%速度旋转补偿项
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;
% 计算位置坐标：
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;
%%循环更新
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);
%计算姿态角已经换算成为角度值
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);
%保存姿态阵的值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);
%结束保存
%%%%保存角速率信息
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('姿态角分量(度)');
% figure;
% plot(l*180/pi,'r');
% hold on;
% plot(fai*180/pi,'g');
% %hold on;
% %plot(h,'b');
% title('经纬度信息(度)');
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('不对准角信息');
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('测试角度');⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -