c语言姿态解算程序,惯导MATLAB程序姿态解算、位置解算、速度解算

close all;

clear all;

clc;

load a.txt; %读文件，为一个六列矩阵，前三列为陀螺输出角速度，后三列为加计输出

format long  %有效数字十六位

%初始化参数

% vx(1)=0.000048637;

% vy(1)=0.000206947;

% vz(1)=0.007106781;          %初始速度

vx(1)=0;

vy(1)=0;

vz(1)=0;

J(1)=116.344695283*2*pi/360;

L(1)=39.975172*2*pi/360;                     %初始经纬度，经纬度化成弧度

% B(1)=-91.637207*2*pi/360;         %初始航向角   tian

% C(1)=0.120992605*2*pi/360;         %初始俯仰角   dong

% D(1)=0.010445947*2*pi/360;         %初始横滚角   bei %初始姿态角

B(1)=0;

C(1)=0;

D(1)=0;

re=6378245;

Wie=7.27221e-5;

e=1/298.3;  %椭圆度

Ti=1;    %采样时间

j=14;

g=9.78049;

wib_c = a(:1:3)‘;   %将a矩阵里的一至三列放入wib_c

f_c = a(:4:6