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];