function[lati1,height1,longi1,Vel1,q2]=navigation_solution(lati,height,longi,Ve0,q1,wibb1,wibb2,fibb1,fibb2)
%%%%%% 参数 %%%%%%%
R=6378137;e=0.0818;
Rn=R*(1-e^2)/((1-e^2*((sin(lati))^2))^1.5);
Re=R/((1-e^2*((sin(lati))^2))^0.5);
Gn=9.7917;
g=[0 0 Gn]';%3*1列向量,北东地坐标系
T=0.010000;
T2=T*2;
wie=7.292115000000000e-05;
wien=[wie*cos(lati) 0 -wie*sin(lati)]'; %3*1列向量,计算wien，北东地坐标系
wenn=[Ve0(2)/(Re+height) -Ve0(1)/(Rn+height) -Ve0(2)*(sin(lati)/cos(lati))/(Re+height)]'; %计算wenn
winn=wien+wenn; %计算winn，3*1
%%%%%%%%% 速度解算 %%%%%%%%%
dV1=fibb1*T; % 1*3向量
dV2=fibb2*T;
dV=dV1+dV2; %计算相对i系速度增量，1*3
Cbn=qua2dcm(q1);
winb=Cbn'*winn; %3*1
dtheta1=wibb1-winb'*T; %1*3向量
dtheta2=wibb2-winb'*T;
dtheta=dtheta1+dtheta2; %计算相对n系角度增量
dVrot=0.5*cross(dV,dtheta);%旋转效应项
dVscul=(cross(dV1,dtheta2)+cross(dV2,dtheta1))*2/3;%划摇效应项
dVsfk=Cbn*(dV+dVrot+dVscul)';%比力积分增量,3*1
dVgcork=(g-cross((2*wien+wenn),Ve0'))*T2; %重力和哥氏加速度积分增量，3*1
Vel1=Ve0'+dVsfk+dVgcork; %更新速度 3*1
%%%% 位置解算 %%%%
lati1=lati+(Vel1(1)+Ve0(1))/(2*(Rn+height))*T2;
longi1=longi+(Vel1(2)+Ve0(2))/(2*(Rn+height)*cos(lati))*T2;
%height1=height+(Vel1(3)+Ve0(3))*0.5*T2;
height1=0;
%%%%%% 姿态解算 %%%%%%%
rotating_vector=dtheta1+dtheta2+cross(dtheta1,dtheta2)*2/3;%等效旋转矢量
rotating_vector1=norm(rotating_vector);
qh=[cos(rotating_vector1/2) rotating_vector(1)/rotating_vector1*sin(rotating_vector1/2) rotating_vector(2)/rotating_vector1*sin(rotating_vector1/2) rotating_vector(3)/rotating_vector1*sin(rotating_vector1/2)]';
q2=quamul(q1,qh);%四元数相乘
q2=real(q2)+imag(q2);
end
