MATLAB编程四元数法捷联惯性导航姿态解算程序源码 - matlab算法设计

下载频道> 资源分类> matlab源码> 算法设计> MATLAB编程四元数法捷联惯性导航姿态解算程序源码

标题：MATLAB编程四元数法捷联惯性导航姿态解算程序源码
分享到：

所属分类: 算法设计资源类型：程序源码文件大小: 384.79 KB 上传时间: 2019-12-01 23:07:41 下载次数: 4 资源积分：1分提供者: jiqiren 20191201110833604

内容：
%四元数法捷联惯性导航姿态解算程序

%close all;

%clear all;

%重力产生的加速度矢量

g=9.8;%单位9.8m/s^2

G=[0,0,-g]';

%G = [0;0;0];

accelerate = G;

n_point = length(G);

dt=0.01;

tm=0:dt:dt*(n_point-1);

%测试数据

accelerate=zeros(3,n_point);

% 陀螺仪数据

data_gyro=zeros(3,n_point);

% 重力轴始终有加速度

accelerate(3,:)=accelerate(3,:)-9.8;

figure

plot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.');

title('加速度计的采样曲线');

legend('x_ACC','Y_ACC','Z_ACC');

xlabel('Time / (10ms)');

ylabel('Accelerate/ (m/s'')');

grid on;

%画出陀螺仪的采样曲线

figure

plot(tm,data_gyro(1,:),'r-',tm,data_gyro(2,:),'g.',tm,data_gyro(3,:),'b-.');

title('陀螺仪的采样曲线');

legend('x_Gyro','Y_Gyro','Z_Gyro');

xlabel('Time / (10ms)');

ylabel('Angel_rate/ (rad/s)');

grid on;

%转移矩阵--即方向余弦矩阵

T=eye(3); %T是3*3的单位矩阵,初始转移矩阵

%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，

%是单位矩阵，利用它们之间的关系确定四元数的初始值。

Q=zeros(4,1);

Q(1)=0.5*sqrt(1+T(1,1)+T(2,2)+T(3,3));

Q(2)=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));

Q(3)=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));

Q(4)=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));

%参见捷联惯性导航技术31页3.64式在旋转90度时不适用

%Q(1,1)=0.5*sqrt(1+T(1,1)+T(2,2)+T(3,3));

%Q(2,1)=1/4/Q(1,1)*(T(3,2)-T(2,3));

%Q(3,1)=1/4/Q(1,1)*(T(1,3)-T(3,1));

%Q(4,1)=1/4/Q(1,1)*(T(2,1)-T(1,2));

%求姿态角矩阵

ANGLE=zeros(3,n_point);%angle[1]代表绕X轴转过的角度，2代表Y轴,3代表Z轴

%方向余弦矩阵到欧拉角的转换关系，这里注意旋转顺序是Z-Y-X，

%参考文献<<一种全新的全角度元元数与欧拉角的转换算法>>

%位置矩阵

position=zeros(3,n_point); %位置矩阵

velocity=zeros(3,n_point);

%速度矩阵

%重力加速度

%acc_g=[0,0,-9.8];

r=[0,0,0,0];

for i=1:n_point %开始循环

if i>1

% velocity(:,i)=((T*accelerate(:,i-1)/2+T*accelerate(:,i))/2+G)*dt+velocity(:,i-1);%要考虑到重力的影响，假定重量方向与子轴方向一致

% position(:,i)=position(:,i-1)+(velocity(:,i-1)+velocity(:,i))*dt/2;

velocity(:,i)=velocity(:,i-1)+T*accelerate(:,i-1)*dt;

position(:,i)=position(:,i-1)+velocity(:,i-1)*dt;

end

%计算欧拉角,假定俯仰角在+_90度范围移动，而滚动角和偏航角在+-180度范围内取值

%ANGLE(1,i)=atan(T(2,3)/T(3,3));

%ANGLE(2,i)=asin(-T(1,3));

%ANGLE(3,i)=atan(T(1,2)/T(1,1));

%if T(3,3)>0 %根据物理意义不可能出现0

% ANGLE(1,i)=-atan(T(2,3)/T(3,3));

%else

% ANGLE(1,i)=-pi*sign(T(2,3))-atan(T(2,3)/T(3,3));

%end

%俯仰角

%ANGLE(2,i)=-asin(-T(1,3));

%偏航角

%if T(1,1)>0%公式似乎有误，直接按公式计算是负值

% ANGLE(3,i)=-atan(T(1,2)/T(1,1));

%else

% ANGLE(3,i)=-pi*sign(T(1,2))-atan(T(1,2)/T(1,1));

%end

ANGLE(1,i)=atan(T(3,2)/T(3,3));

ANGLE(2,i)=asin(-T(3,1));

ANGLE(3,i)=atan(T(2,1)/T(1,1));

%更新四元数

if i<n_point %如果还没有到超出数组范围

theta=data_gyro(:,i)*dt;%角度向量

%换用简化算法试验结果

r = [1 0.5*theta(1) 0.5*theta(2) 0.5*theta(3)];

% ang_vect = theta;

% magn = norm(ang_vect);

% if magn == 0,

% r = [1 0 0 0];

% else

% ac = cos(magn/2);

% as = ( sin(magn/2) )/magn;

% r = [ac as*ang_vect(1) as*ang_vect(2) as*ang_vect(3)];

% end

Q = [r(1),-r(2),-r(3),-r(4);

r(2), r(1), r(4),-r(3);

r(3),-r(4), r(1), r(2);

r(4), r(3),-r(2), r(1)]*Q;

%更新方向余弦矩阵T(t,b)

T=[1-2*(Q(3)*Q(3)+Q(4)*Q(4)) 2*(Q(2)*Q(3)-Q(1)*Q(4)) 2*(Q(2)*Q(4)+Q(1)*Q(3));

2*(Q(2)*Q(3)+Q(1)*Q(4)) 1-2*(Q(2)*Q(2)+Q(4)*Q(4)) 2*(Q(3)*Q(4)-Q(1)*Q(2));

2*(Q(2)*Q(4)-Q(1)*Q(3)) 2*(Q(3)*Q(4)+Q(1)*Q(2)) 1-2*(Q(2)*Q(2)+Q(3)*Q(3))];

%得到姿态矩阵

end

figure

ANGLE=ANGLE*180/pi;

plot(tm,ANGLE(1,:),'r-',tm,ANGLE(2,:),'g.',tm,ANGLE(3,:),'b-.');

legend('Pitch Angel','Roll Angel','Yaw Angel');

title('Gesture Calculation');

xlabel('Time / (10ms)');

ylabel('Angel/ (degree)');

grid on;

figure

plot(tm,velocity)

legend('Navigation Coordinate: X','Navigation Coordinate: Y','Navigation Coordinate: Z');

title('Velocity Calculation');

xlabel('Time / (10ms)');

ylabel('Velocity/ (m/s)');

grid on;

figure

%plot(tm,position);

%legend('Navigation Coordinate: X','Navigation Coordinate: Y','Navigation Coordinate: Z');

%title('Position Calculation');

%xlabel('Time / (10ms)');

%ylabel('Position/ (m)');

%grid on;

plot3(position(1,:),position(2,:),position(3,:));

grid on

axis equal

title('Position Calculation');

xlabel('east (meters)')

ylabel('north (meters)')

zlabel('up (meters)')

%roll：横滚角　x轴

%pitch:俯仰解：　y axis

%yaw偏航角 z axis

文件列表（点击上边下载按钮，如果是垃圾文件请在下面评价差评或者投诉）:

MATLAB编程四元数法捷联惯性导航姿态解算程序源码/
MATLAB编程四元数法捷联惯性导航姿态解算程序源码/insdem2_data.mat
MATLAB编程四元数法捷联惯性导航姿态解算程序源码/instest.m

关键词： MATLAB 编程 四元数法 捷联 惯性 导航 姿态 解算 程序 源码

matlab源码下载排行

回到顶部

友情链接

matlab源码网站源码编程语言网络编程手机通讯编程 KUNAG 官网 KUNAG 电商更多

: 联系方式| 版权声明| 招聘信息| 广告服务| 银行汇款| 法律顾问| 兼职技术| 付款方式| 关于我们|; 网站客服程序员兼职招聘 411241577@qq.com; 沪ICP备19040327号-3; 公安备案号：沪公网安备 31011802003874号; 库纳格流体控制系统（上海）有限公司版权所有; Copyright © 1999-2014, GUSUCODE.COM, All Rights Reserved

关键词： MATLAB 编程 四元数法 捷联 惯性 导航 姿态 解算 程序 源码

相关推荐

matlab源码下载排行

友情链接

关键词： MATLAB 编程四元数法捷联惯性导航姿态解算程序源码