• 鱼鱼鱼郭敬明
    了解作者
  • matlab
    开发工具
  • 2KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 10 积分
    下载积分
  • 3
    下载次数
  • 2019-01-02 17:30
    上传日期
一个简单的惯性导航模拟实现,实现计算姿态角,速度和经纬高的功能
D4.rar
  • D4.m
    8.9KB
内容介绍
fileID=fopen('Data1.bin'); fileID1=fopen('Data1_PureINS.bin'); Z=fread(fileID,[7,904601],'double'); Z1=fread(fileID1,[10,720200],'double'); Z=Z(:,184402:904601) pi=3.141592653589793; %圆周率 r=0.0107951084511778; %横滚角 p=-2.14251290749072; %俯仰角 h=-75.7498049314083; %航向角 V=[0 0 0]'; %初始速度 Wie=7.29211e-5; %地球自转角速度 T=0.005; %采样间隔 B=113.3713651222; %初始纬度 lat=23.1373950708; %初始经度 e2=0.00669437990; %WGS84坐标椭球第一偏心率的平方 a=6378137.0000; %WGS84坐标椭球长半径 M=a*(1-e2)/(1-e2*sin(B)^2)^(3/2); %子午圈曲率半径 N=a/sqrt(1-e2*sin(B)^2); %卯酉圈曲率半径 h1=2.175; %初始高度 I=eye(3); %单位矩阵 %当地重力加速度 g=9.7803267715*(1.0 + 0.0052790414*sin(lat)*2+0.0000232718*sin(lat)*4)+(-0.000003087691089+0.000000004397731*sin(lat)*2)*h + 0.000000000000721*h1*h1; Gx1=Z(2,1); Gy1=Z(3,1); Gz1=Z(4,1); Ax1=Z(5,1); Ay1=Z(6,1); Az1=Z(7,1); Vk1=[Ax1 Ay1 Az1]; Gk1=[Gx1 Gy1 Gz1]; V1=[0 0 0]'; %姿态更新 for n=2:720200 r=Z(2,n); p=Z(3,n); h=Z(4,n); qbn=[cos(r/2)*cos(p/2)*cos(h/2)+sin(r/2)*sin(p/2)*sin(h/2) sin(r/2)*cos(p/2)*cos(h/2)-cos(r/2)*sin(p/2)*sin(h/2) cos(r/2)*sin(p/2)*cos(h/2)+sin(r/2)*cos(p/2)*sin(h/2) cos(r/2)*cos(p/2)*sin(h/2)-sin(r/2)*sin(p/2)*cos(h/2)]'; q0=cos(r/2)*cos(p/2)*cos(h/2)+sin(r/2)*sin(p/2)*sin(h/2); q1=sin(r/2)*cos(p/2)*cos(h/2)-cos(r/2)*sin(p/2)*sin(h/2); q2=cos(r/2)*sin(p/2)*cos(h/2)+sin(r/2)*cos(p/2)*sin(h/2); q3=cos(r/2)*cos(p/2)*sin(h/2)-sin(r/2)*sin(p/2)*cos(h/2); Cbn=[q0^2+q1^2-q2^2-q3^2 2*(q1*q2-q0*q3) 2*(q1*q3+q0*q2) 2*(q1*q2+q0*q3) q0^2-q1^2+q2^2-q3^2 2*(q2*q3-q0*q1) 2*(q1*q3-q0*q2) 2*(q2*q3+q0*q1) q0^2-q1^2-q2^2+q3^2]; r(n)=atan(Cbn(2,1)/Cbn(1,1)); %航向角 p(n)=atan(-Cbn(3,1)/(Cbn(3,2)^2+Cbn(3,3)^2)^(1/2)); %俯仰角 h(n)=atan((Cbn(3,2)/Cbn(3,3))); %横滚角 %速度更新 Gx=Z(2,n); Gy=Z(3,n); Gz=Z(4,n); Ax=Z(5,n); Ay=Z(6,n); Az=Z(7,n); Vk=[Ax Ay Az]; Gk=[Gx Gy Gz]; Vkb=Vk+(1/2)*cross(Gk,Vk)+(1/12)*(cross(Gk1,Vk)+cross(Vk1,Gk)); Vk1=[Ax Ay Az]; Gk1=[Gx Gy Gz]; %存储上一历元信息吗, Wien=[Wie*cos(B) 0 -Wie*sin(B)]'; c=tan(B)*power(N+h1,-1); o=V(2)*power((N+h1),-1); Wen=[V(2)*power((N+h1),-1) V(1)*power((M+h1),-1) -V(2)*power(c,-1)]'; L=(Wien+Wen)*T; L1=[0 -L(3) L(2);L(3) 0 -L(1);-L(2) L(1) 0]; Vkb=Vkb'; Vfk=(I-0.5*L1)*Cbn*Vkb; g1=[0,0,g]'; Vgn=(g1-cross((2*Wien+Wen),V))*T; %位置更新 M1=M; N1=N; B1=B; B2(n)=B; N2(n)=N; lat1=lat; M=a*(1-e2)/(1-e2*sin(B1)^2)^(3/2); %子午圈曲率半径更新 H(n)=h1-(V(3)+V1(3))*T*0.5; %高度更新 h1(n)=(H(n)+h1)*0.5; B=B1+T*(1/2)*(V(1)+V1(1))*power((M1+h1(n)),-1); %纬度更新 N1= a/sqrt(1-e2*sin(B1)^2); %经度更新 Bx=(B+B1)/2; lat=lat1+T*(1/2)*(V(2)+V1(2))/(N1+h1(n)*cos(Bx)); %卯酉圈曲率半径更新 V1=V; %速度更新 h1=h1(n); V2(n-1)=V(1); V3(n-1)=V(2); V4(n-1)=V(3); V=V+Vgn+Vfk; end t1=2:720200; figure(1); p1(t1)=p(t1)*pi/180; plot(t1/200,p1(t1),'g'); xlabel('Time(second)'); ylabel('Angle(degree)'); title('俯仰角'); grid on; %俯仰角绘图 figure(2); h1(t1)=h(t1)*pi/180; plot(t1/200,h1(t1),'b'); xlabel('Time(second)'); ylabel('Angle(degree)'); title('横滚角'); grid on; %横滚角绘图 figure(3); r1(t1)=r(t1)*pi/180; plot(t1/200,r1(t1),'r'); xlabel('Time(second)'); ylabel('Angle(degree)'); title('航向角'); grid on; %航向角绘图 figure(4); plot(t1/200,V2(t1),'g'); xlabel('Time(second)'); ylabel('V(m/s)'); title('Pitch(北向速度)'); grid on; %北向速度绘图 figure(5); plot(t1/200,V3(t1),'b'); xlabel('Time(second)'); ylabel('Vy(m/s)'); title('东向速度'); grid on; %东向速度绘图 figure(6); plot(t1/200,V4(t1),'r'); xlabel('Time(second)'); ylabel('Vz(m/s)'); title('垂向速度'); grid on; %垂向速度绘图 figure(13); B2(t1)=B2(t1)*pi/180; plot(t1/200,B2(t1),'r'); xlabel('Time(second)'); ylabel('度'); title('纬度'); %纬度绘图 figure(14); N2(t1)=N2(t1)*pi/180; plot(t1/200,N2(t1),'r'); xlabel('Time(second)'); ylabel('度'); title('经度'); %经度绘图 grid on; figure(7); Z1(9,t1)=Z1(9,t1)*pi/180; plot(t1/200,Z1(9,t1),'g'); xlabel('Time(second)'); ylabel('Angle(degree)'); title('俯仰角'); grid on; %俯仰角绘图 figure(8); Z1(8,t1)=Z1(8,t1)*pi/180; plot(t1/200,Z1(8,t1),'b'); xlabel('Time(second)'); ylabel('Angle(degree)'); title('横滚角'); grid on; %横滚角绘图 figure(9); Z1(10,t1)=Z1(10,t1)*pi/180; plot(t1/200,Z1(10,t1),'r'); xlabel('Time(second)'); ylabel('Angle(degree)'); title('航向角'); grid on; %航向角绘图 figure(10); plot(t1/200,Z1(5,t1),'g'); xlabel('Time(second)'); ylabel('V(m/s)'); title('Pitch(北向速度)'); grid on; %北向速度绘图 figure(11); plot(t1/200,Z1(6,t1),'b'); xlabel('Time(second)'); ylabel('Vy(m/s)'); title('东向速度'); grid on; %东向速度绘图 figure(12); plot(t1/200,Z1(7,t1),'r'); xlabel('Time(second)'); ylabel('Vz(m/s)'); title('垂向速度'); grid on; %垂向速度绘图 figure(15); Z1(2,t1)=Z1(2,t1)*pi/180; plot(t1/200,Z1(2,t1),'r'); xlabel('Time(second)'); ylabel('度'); title('纬度'); %纬度绘图 figure(16); Z1(3,t1)=Z1(3,t1)*pi/180; plot(t1/200,Z1(3,t1),'r'); xlabel('Time(second)'); ylabel('度'); title('经度'); %经度绘图 grid on; figure(23); plot(t1/200,H(t1),'r'); xlabel('Time(second)'); ylabel('h'); title('高度'); %高度绘图 grid on; figure(24); plot(t1/200,Z(4,t1),'r'); xlabel('Time(second)'); ylabel('h'); title('高度'); %高度绘图 grid on; VVx(t1)=V2(t1)-Z(5,t1); %北向速度差值 VVy(t1)=V3(t1)-Z(6,t1); %东向速度差值 VVz(t1)=V4(t1)-Z(7,t1); %垂向速度差值 VVp(t1)=p1(t1)-Z(9,t1); %俯仰角差值 VVh(t1)=h1(t1)-Z(8,t1); %横滚角差值 VVr(t1)=r1(t1)-Z(10,t1); %航向角差值 VVB2(t1)=B2(t1)-Z(2,t1); %纬度差值 VVN2(t1)=N2(t1)-Z(3,t1); %经度差值 VVH(t1)=H(t1)-Z(4,t1); %高度差值 figure(17); plot(t1/200,VVx(t1),'r'); xlabel('Time(second)'); ylabel('m/s'); title('北向速度差值'); %北向速度差值绘图 grid on; figure(18); plot(t1/200,VVy(t1),'r'); xlabel('Time(second)'); ylabel('m/s'); title('东向速度'); %东向速度绘图 grid on; figure(19); plo
评论
    相关推荐
    • codesforimageprocessing.rar
      实现简单图像处理,包括256色转灰度图、Hough变换、Walsh变换、中值滤波、二值化变换、亮度增减、傅立叶变换、反色、取对数、取指数、图像平移、图像旋转、图像细化、图像缩放、图像镜像、均值滤波、对比度拉伸、拉普拉斯锐化(边缘检测)、方块编码、梯度锐化、灰度均衡、直方图均衡、离散余弦变换、维纳滤波处理、逆滤波处理、阈值变换、高斯平滑。
    • matlab.rar
      基于MATLAB-GUI图形界面的数字图像处理软件 本系统设计基于GUI图形界面,用matlab语言编写代码,实现功能包括图象的读取、存储、显示、直方图均衡化、阈值化、小波分解、小波重构、加噪、去噪、平滑、锐化、边缘检测、图像分割等
    • OPENCV_SIFT_VC6.rar
      基于OPENCV的SIFT特征提取与匹配算法。包含完整的从图像高斯金字塔、DOG、空间极值点提取、关键点描述、KDtree匹配等关键步骤的全部函数实现,对全面深入理解Lowe的SIFT算法有莫大帮助。程序运行前须安装(1)OpenCV: http://opencvlibrary.sourceforge.net (2)SIFT: http://web.engr.oregonstate.edu/~hess/index.html,并配置其环境参数。
    • MATLAB_code_of_image_processing.rar
      该gui函数基本上包括图像处理里面的最基本处理,相当于一个小型photoshop。比如读取文件,几何变换中的垂直镜像,平移,旋转,缩放;正交变换的DFT,FFT,DCT,DST,DHT,DWashT;灰度处理中的反色,直方图均衡,全局线性变换,分段线性变换,指数非线性变换,对数非线性变换;图像增强里面的加噪声,平滑,锐化,伪彩色增强;图像分割里面的灰度阈值法,Robert,Laplace,sobel,prewitt,canny算子边缘检测法;图像恢复里面的直接逆滤波,维纳滤波;图像编码里面的霍夫曼编码,行程编码等等
    • bianyuan_jiance_bijiao.rar
      用六种算子(分别是gabor、拉普拉斯、priwitt、robert、sobel、wallis),对三种图象进行边缘检测比较,强烈推荐哦,是本人的毕业设计中的关键程序
    • 一维和二维小波变换的VC实现.rar
      小波算法在vc的具体实现.小波算法在图像处理,视频压缩,特征提取等领域有广泛应用,程序以灰度图像为数据分别演示了一维和二维小波变换,希望对大家有所帮助.谢谢!
    • thresold.rar
      基于小波变换阈值去噪的MATLAB源代码(包含小波包去噪程序)
    • HOG.rar
      基于HOG的行人检测,作者的源代码有错误,现代码已经进行了改变,并可以调试通
    • tuxiangzengqiang.rar
      本程序基于Matlab,初步实现图像增强的基本操作,实现 图像明暗度调整, 图像旋转,更改颜色显示方式,直接灰度调整,图像求反,动态范围压缩,显示原始图像直方图,直方图均衡化,滤波增强等功能
    • ImgSeg.rar
      哥们图像边缘检测与特征提取的毕业设计,包括全部经典算法,还有他自己的改进,VC6下测试通过。(这个改进我不是很懂,不要问我!:))