惯导MATLAB程序（姿态解算、位置解算、速度解算）171777

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）;        %卯酉圈曲率半径
    Ry=re/（1+2*e-3*e*sin（L（1））^2）;  %子午圈主曲率半径
  
    q（1） = cos（B（1）/2）*cos（C（1）/2）*cos（D（1）/2）-sin（B（1）/2）*sin（C（1）/2）*sin（D（1）/2）;
    q（2） = cos（B（1）/2）*sin（C（1）/2）*cos（D（1）/2）-sin（B（1）/2）*cos（C（1）/2）*sin（D（1）/2）;
    q（3） = cos（B（1）/2）*cos（C（1）/2）*sin（D（1）/2）+sin（B（1）/2）*sin（C（1）/2）*cos（D（1）/2）;
    q（4） = cos（B（1）/2）*sin（C（1）/2）*sin（D（1）/2）+sin（B（1）/2）*cos（C（1）/2）*cos（D（1）/2）;
    
    T11 = （q（1））^2 + （q（2））^2 - （q（3））^2 - （q（4））^2;
    T12 = 2*q（3）*q（2） + 2*q（1）*q（4）; 
    T13 = 2*q（4）*q（2） - 2*q（1）*q（3）;
    T21 = 2*q（3）*q（2） - 2*q（1）*q（4）;
    T22 = （q（1））^2 - （q（2））^2 + （q（3））^2 - （q（4））^2;
    T23 = 2*q（3）*q（4） - 2*q（1）*q（2）;
    T31 = 2*q（4）*q（2） + 2*q（1）*q（3）;
    T32 = 2*q（3）*q（4） - 2*q（1）*q（2）;
    T33 = （q（1））^2 - （q（2））^2 - （q（3））^2 + （q（4））^2;       
    

    Cbn=[T11 T12 T13;T21 T22 T23;T31 T32 T33];%粗对准后确定的姿态矩阵
    Cnb=Cbn‘;
    
    gnx=0;
    gny=0;
    gnz=g;
    gn=[gnx;gny;gnz];%3*1
     gb=gn‘*Cbn;   %重力加速度在机体系的表示 1*3*3*3=1*3
    Wien_x=0;
    Wien_y=Wie*cos（L（1））;
    Wien_z=Wie*sin（L（1））;
    Wien=[Wien_x;Wien_y;Wien_z];%3*1