惯导MATLAB程序姿态解算、位置解算、速度解算.m

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）‘;      %将a矩阵里的一至三列放入f_c
wib_x = a（:1）;
wib_y = a（:2）;
wib_z = a（:3）;
fx = a（:4）;
fy = a（:5）;
fz = a（:6）;

for i=1:j;
    Rx=re/（1-e*sin（L（1））^2）