文档章节

Matlab 飞机航向INS仿真

Pulsar-V
 Pulsar-V
发布于 2017/10/04 08:30
字数 836
阅读 84
收藏 0
close all;
clear all;
clc;
format long
%初始化参数
%vx(1)=0.000048637;
%vy(1)=0.000206947;
%vz(1)=0.007106781;          %初始速度
vx(1)=0;
vy(1)=0;
vz(1)=0;
jd(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/20;
j=100;
N=200;
g=9.78049;

    Wibxb_INSc=randn(j,1);
    Wibyb_INSc=randn(j,1);
    Wibzb_INSc=randn(j,1);%陀螺在各个方向上测量的数据
    Fbx_INSc=randn(j,1);
    Fby_INSc=randn(j,1);
    Fbz_INSc=randn(j,1);%加速度计在各个方向上测量的数据
for i=1:j
    Rx=re/(1-e*sin(L(1))^2);
    Ry=re/(1+2*e-3*e*sin(L(1))^2);
  
    T31=Fbx_INSc(i,1)/g*Ti;
    T32=Fby_INSc(i,1)/g*Ti;
    T33=Fbz_INSc(i,1)/g*Ti;
    T21=(Wibxb_INSc(i,1)-Ti*T31*Wie*sin(L(1)))/Ti*Wie*cos(L(1));
    T22=(Wibyb_INSc(i,1)-Ti*T32*Wie*sin(L(1)))/Ti*Wie*cos(L(1));
    T23=(Wibzb_INSc(i,1)-Ti*T33*Wie*sin(L(1)))/Ti*Wie*cos(L(1));
    T11=T22*T33-T23*T32;
    T12=T23*T31-T21*T33;
    T13=T21*T32-T22*T31;   
    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
    Wieb=Wien'*Cbn; %地球自转角速度在机体系的表示
    Wibb=[Wibxb_INSc(i,1) Wibyb_INSc(i,1) Wibzb_INSc(i,1)]';%陀螺输出的各个轴表示
    Fb=[Fbx_INSc(i,1) Fby_INSc(i,1) Fbz_INSc(i,1)]';  %加速度计输出的各个轴表示
    %Wibb'=Wien'*Cbn;
    %[Fbx_INSc(i,1) Fby_INSc(i,1) Fbz_INSc(i,1)]=-1*gn'*Cbn;
  %姿态角的计算
    if abs(Cnb(2,2))>1e-10
        if Cnb(2,2)>0
           B(i+1)=atan(Cnb(2,1)/Cnb(2,2));              
        elseif Cnb(2,1)>0
           B(i+1)=atan(Cnb(2,1)/Cnb(2,2))+pi;
        else 
            B(i+1)=atan(Cnb(2,1)/Cnb(2,2))-pi;
        end
    elseif Cnb(2,1)>0
       B(i+1)=pi/2;
    else 
       B(i+1)=-pi/2;
    end                     %求航向角
   
    C(i+1)=asin(Cnb(2,3));   %求俯仰角
    if abs(Cnb(3,3))>1e-10
        if Cnb(3,3)>0
           D(i+1)=atan(-Cnb(1,3)/Cnb(3,3));
        elseif Cnb(1,3)>0
            D(i+1)=atan(-Cnb(1,3)/Cnb(3,3))-pi;
        else
           D(i+1)=atan(-Cnb(1,3)/Cnb(3,3))+pi;
        end
   elseif Cnb(1,3)>0
       D(i+1)=-pi/2;
   else 
       D(i+1)=pi/2;
   end                   %求横滚角
   
    vx(i+1)=(Fbx_INSc(i,1)+2*Wie*sin(L(1))*vy(i)+vx(i)*vy(i)*tan(L(1))/Rx-2*Wie*cos(L(1))*vz(i)-vx(i)*vz(i)/Rx)*Ti+vx(i);%东向速度
    vy(i+1)=(Fby_INSc(i,1)-2*Wie*sin(L(1))*vx(i)-vx(i)*vx(i)*tan(L(1))/Rx-vy(i)*vz(i)/Rx)*Ti+vy(i);%北向速度
    vz(i+1)=(Fbz_INSc(i,1)+(2*Wie*cos(L(1))+vx(i)/Rx)*vx(i)+vy(i)*vy(i)/Ry-g)*Ti+vz(i);%天向速度
    %L(i+1)=vy(i)*Ti/Ry+L(i);                    %纬度
    %jd(i+1)=vx(i)*Ti/(Rx*cos(L(i)))+jd(i);        %经度
     
    

end


 t=0:0.1:j;
  %figure(1)
  %plot(jd,'r');xlabel('时间'),ylabel('经度');
  %figure(2)
  %plot(L,'g');xlabel('时间'),ylabel('纬度');
  figure(3)
  plot(vx,'b');xlabel('时间'),ylabel('东向速度');
  figure(4)
  plot(vy,'b');xlabel('时间'),ylabel('北向速度');
  figure(5)
  plot(vz,'b');xlabel('t'),ylabel('天向速度');
  figure(6)
  plot(B,'b');xlabel('时间'),ylabel('航向角');
  figure(7)
  plot(C,'g');xlabel('时间'),ylabel('俯仰角');
  figure(8)
  plot(D,'r');xlabel('时间'),ylabel('横滚角');

© 著作权归作者所有

Pulsar-V

Pulsar-V

粉丝 64
博文 153
码字总数 136215
作品 1
成都
架构师
私信 提问
除了不会生孩子 MATLAB 什么都会

Part 1 有趣的 MATLAB 如果你在暑假、有WIFI、有西瓜的空调房里不知道做什么,不如学下MATLAB吧,有了它,你可以什么都不需要! 知乎网友@邵恩真,示范了如何使用花式 MATLAB 玩游戏! 用 MA...

21ic电子网
2018/02/01
0
0
MatLab 组件大全

MATLAB 矩阵实验室 7.0.1 Simulink 仿真 6.1 Aerospace Blockset 太空模块 1.6.1 Bioinformatics Toolbox 生物信息工具箱 1.1.1 CDMA Reference Blockset 码分多址参数模块 1.1 Communicatio......

机器的心脏
2017/12/15
0
0
Mathworks资深应用工程师王鸿钧:仿真加速自动驾驶功能的开发 | 2018全球智能驾驶峰会

Mathworks资深应用工程师 王鸿钧 雷锋网新智驾按:10月26日至27日,2018 全球智能驾驶峰会在苏州召开,本次峰会由苏州市相城区人民政府主办,苏州高铁新城管理委员会、雷锋网&新智驾和数域承...

陈小同
2018/10/29
0
0
matlab-线性代数 向量单位化

  matlab : R2018a 64bit     OS : Windows 10 x64 typesetting : Markdown    blog : my.oschina.net/zhichengjiu    gitee : gitee.com/zhichengjiu   code   result   resour......

志成就
2019/02/21
122
0
matlab-自控原理 laplace、ilaplace 拉氏、拉氏逆变换

  matlab : R2018a 64bit     OS : Windows 10 x64 typesetting : Markdown    blog : my.oschina.net/zhichengjiu    gitee : gitee.com/zhichengjiu   拉氏变换 code result   ......

志成就
2019/02/23
36
0

没有更多内容

加载失败,请刷新页面

加载更多

如何为MVC-3转换为4应用程序添加对System.Web.Optimization的引用

我正在最近从MVC 3转换为MVC 4 beta的项目中尝试使用新的捆绑功能。 它需要global.asax中的一行代码, BundleTable.Bundles.RegisterTemplateBundles(); ,这需要using System.Web.Optimiza...

技术盛宴
今天
61
0
Kettle自定义jar包供javascript使用

我们都知道 Kettle 是用 Java 语言开发,并且可以在 JavaScript 里面直接调用 java 类方法。所以有些时候,我们可以自定义一些方法,来供 JavaScript 使用。 本篇文章有参考自:https://www...

CREATE_17
昨天
106
0
处理CSV文件中的逗号

我正在寻找有关如何处理正在创建的csv文件的建议,然后由我们的客户上传,并且该值可能带有逗号(例如公司名称)。 我们正在研究的一些想法是:带引号的标识符(值“,”值“,”等)或使用|...

javail
昨天
79
0
计算一个数的数位之和

计算一个数的数位之和 例如:128 :1+2+8 = 11 public int numSum(int num) { int sum = 0; do { sum += num % 10; } while ((num = num / 10) > 0); return sum;......

SongAlone
昨天
128
0
为什么图片反复压缩后普遍会变绿,而不是其他颜色?

作者:Lion Yang 链接:https://www.zhihu.com/question/29355920/answer/119088684 来源:知乎 著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。 业余版概要:安卓的...

shzwork
昨天
85
0

没有更多内容

加载失败,请刷新页面

加载更多

返回顶部
顶部