www.gusucode.com > 惯性导航系统仿真程序 SIMULINK环境-SIMULINK源码程序 > code/dao_hang.m
function gyg=dao_hang(SINS)
global frame wie sampt0
global r0 Lati0 Longi0 Lati Longi eeee
global Vn Vu Ve cbn Q Alti CC cnb
global X A B K P Fn d_e d_v ang_1 ang_2 vel_1 vel_2
global d_roll d_yaw d_pitch navi_resul
frame = frame + 1;
d_angle = SINS(5:7)*0.01;
d_vel = SINS(2:4)*0.01;
% Gyg = [d_roll d_yaw d_pitch];
% d_anV = d_angle*100.0;
% d_cnb = eulr2dcm(d_angle);
% cnb = cnb*d_cnb;
% cbn = cnb';
gyg = [navi_resul(2) navi_resul(3) navi_resul(4)];
% gyg(1) = cbn(1,1); gyg(2) = cbn(2,2); gyg(3) = cbn(3,3);
gyg = [ navi_resul(2:4)'; d_e*3600*180/pi; d_v*1e6/9.8];
flag = 1;
if flag>0
if mod(frame,2)~=0
ang_1 = d_angle + d_e*0.01;
d_vell = d_vel;
vel_1(1) = d_vell(1) - d_v(1)*0.01;
vel_1(2) = d_vell(2);
vel_1(3) = d_vell(3) - d_v(2)*0.01;
else
ang_2 = d_angle+ d_e*0.01;
dd_vel = d_vel;
vel_2(1) = dd_vel(1) - d_v(1)*0.01;
vel_2(2) = dd_vel(2);
vel_2(3) = dd_vel(3) - d_v(2)*0.01;
angle = ang_1 + ang_2;
velocity = vel_1 + vel_2;
%划船补偿
vel_scull_b = velocity + 0.5 * cross(angle,velocity)...
+ (2.0/3.0)*(cross(ang_1,vel_2) + cross(vel_1,ang_2));
vel_scull_n = cbn * vel_scull_b;
wien = [ wie*cos(Lati) wie*sin(Lati) 0]';
temp = 1 - eeee * sin(Lati)^2;
rn = r0 * (1-eeee)/temp^1.5;
re = r0/temp^0.5;
wenn = [Ve/re Ve*tan(Lati)/rn -Vn/rn]';
g_u = -9.7803267711905*(1+0.00193185138639*sin(Lati)^2)...
/((1-0.00669437999013*sin(Lati)^2)^0.5*(1.0 + Alti/r0));
g = [0 g_u 0]';
V_last = [Vn Vu Ve]';
wien2_wenn_V = cross((2.0*wien + wenn),V_last);
Vel_update = V_last + vel_scull_n + 2.0*sampt0*(g-wien2_wenn_V);
Vn = Vel_update(1);
Vu = Vel_update(2) * 0;
Ve = Vel_update(3);
Lati = Lati + 2.0*sampt0*Vn/rn;
Longi = Longi + 2.0*sampt0*Ve/(re*cos(Lati));
Alti = 0;
wenn = [Ve/re Ve*tan(Lati)/rn -Vn/rn ]';
winn = wien + wenn;
winb = cbn' * winn;
%双子样等效转动矢量计算
ang_1 = ang_1 - winb * sampt0;
ang_2 = ang_2 - winb * sampt0;
TurnVector = ang_1 + ang_2 + (2.0/3.0)*cross(ang_1,ang_2);
%计算四元数
NormSquare = TurnVector' * TurnVector ;
d_M1 = [0 -TurnVector(1) -TurnVector(2) -TurnVector(3)];
d_M2 = [TurnVector(1) 0 TurnVector(3) -TurnVector(2)];
d_M3 = [TurnVector(2) -TurnVector(3) 0 TurnVector(1)];
d_M4 = [TurnVector(3) TurnVector(2) -TurnVector(1) 0];
d_M = [d_M1; d_M2; d_M3; d_M4];
Q_M = ( 1 - NormSquare/8.0 + NormSquare^2/384.0 ) * eye(4)...
+ (0.5 - NormSquare/46.0) * d_M;
Q = Q_M * Q;
Q = Q/norm(Q);
cnb = quat2dcm(Q);
cbn = cnb';
navi_resul(1) = frame/100;
navi_resul(2) = atan2(-cnb(3,2),cnb(2,2))*180/pi;
navi_resul(3) = atan2(-cnb(1,3),cnb(1,1))*180/pi;
navi_resul(4) = asin(cnb(1,2))*180/pi;
navi_resul(5) = Vn;
navi_resul(6) = Ve;
navi_resul(7) = (Lati - Lati0) *rn;
navi_resul(8) = (Longi - Longi0) * re *cos(Lati);
Fn = cbn*(velocity/2.0)/sampt0;
A(1,2) = -2.0*( wie*sin(Lati) + Ve*tan(Lati)/rn);
A(1,4) = -Fn(3);
A(1,5) = Fn(2);
A(2,1) = 2.0*wie*sin(Lati) + Ve*tan(Lati)/rn;
A(2,2) = Vn*tan(Lati)/rn;
A(2,3) = -Fn(2);
A(2,4) = Fn(1);
A(3,2) = 1.0/re;
A(3,4) = -Vn/rn;
A(3,5) = -wie*sin(Lati) - Ve*tan(Lati)/rn;
A(4,2) = tan(Lati)/rn;
A(4,3) = Vn/rn;
A(4,5) = wie*cos(Lati)+Ve/re;
A(5,1) = -1.0/rn;
A(5,3) = wie*sin(Lati) + Ve*tan(Lati)/rn;
A(5,4) = -wie*cos(Lati) -Ve/re;
B(1,1) = cbn(1,1);
B(1,2) = cbn(1,3);
B(2,1) = cbn(3,1);
B(2,2) = cnb(3,3);
B(3:5,3:5) = cbn;
AA = [A,B;zeros(5,5),zeros(5,5)];
CC(1,1) = 1.0;
CC(2,2) = 1.0;
Z(1,1) = Vn;
Z(2,1) = Ve;
Q0 = diag([(5e-5)^2 (5e-5)^2 (0.2*pi/180/3600)^2 (0.2*pi/180/3600)^2 (0.2*pi/180/3600)^2]);
R0 = diag([0.00001^2 0.00001^2]);
if(frame==2)
X = zeros(10,1);
P = diag([0.001^2 0.001^2 0.001^2 0.001^2 0.001^2 ...
(5e-4)^2 (5e-4)^2 (1*pi/180/3600)^2 (1*pi/180/3600)^2 (1*pi/180/3600)^2]);
end
BB = zeros(10,5);
BB(1:5,1:5) = B;
dt = sampt0*2.0;
[X,P,K] = KF10(AA,BB,CC,Z,X,P,Q0,R0,dt);
d_Vn = X(1);
d_Ve = X(2);
d_roll = X(3);
d_yaw = X(4);
d_pitch = X(5);
% Gyg = [d_roll d_yaw d_pitch];
if((frame>=0)&&(mod(frame,1e8)==0))
d_e = d_e + X(8:10);
d_v = d_v + X(6:7);
X = zeros(10,1);
Vn = Vn - d_Vn;
Ve = Ve - d_Ve;
d_dcmnb1 = [cos(d_yaw)*cos(d_pitch) sin(d_pitch) -sin(d_yaw)*cos(d_pitch)];
d_dcmnb2 = [sin(d_yaw)*sin(d_roll)-cos(d_yaw)*sin(d_pitch)*cos(d_roll),...
cos(d_pitch)*cos(d_roll),...
cos(d_yaw)*sin(d_roll)+sin(d_yaw)*sin(d_pitch)*cos(d_roll)];
d_dcmnb3 = [sin(d_yaw)*cos(d_roll)+cos(d_yaw)*sin(d_pitch)*sin(d_roll),...
-cos(d_pitch)*sin(d_roll),...
cos(d_yaw)*cos(d_roll)-sin(d_yaw)*sin(d_pitch)*sin(d_roll)];
d_cnb = [d_dcmnb1;d_dcmnb2;d_dcmnb3];
cnb = cnb * d_cnb;
cbn = cnb';
q0(1) = sqrt(1+cnb(1,1)+cnb(2,2)+cnb(3,3))/2;
q0(2) = -(cnb(3,2)-cnb(2,3))/(4*q0(1));
q0(3) = -(cnb(1,3)-cnb(3,1))/(4*q0(1));
q0(4) = -(cnb(2,1)-cnb(1,2))/(4*q0(1));
temp = sqrt(q0(1)*q0(1) + q0(2)*q0(2) + q0(3)*q0(3) + q0(4)*q0(4));
q0(1) = q0(1)/temp;
q0(2) = q0(2)/temp;
q0(3) = q0(3)/temp;
q0(4) = q0(4)/temp;
Q = q0';
OutPut = [navi_resul,d_e',d_v'];
end
end
end
% OutPut = [navi_resul',d_e,d_v];
Gyg = [d_roll d_yaw d_pitch];