资源简介
用四元数法进行姿态解算的matlab程序。
代码片段和文件信息
close all;
clear all;
%重力产生的加速度矢量
g=9.8;%单位9.8m/s^2
G=[00-g]‘;
%****************************读入数据
%**********读入陀螺仪的数据
gyro_x=load(‘gyrox.txt‘);
gyro_y=load(‘gyroy.txt‘);
gyro_z=load(‘gyroz.txt‘);
%****************读入加速度计的数据**************
%acc_rate=3/1024;
acc_x =load(‘acceleratex.txt‘);
acc_y =load(‘acceleratey.txt‘);
acc_z=load(‘acceleratez.txt‘);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%加速度数字转换为模拟电压
data_acc=[acc_x;acc_y;acc_z];
data_acc=data_acc/1024*3
%将数据转换为相应的加速度值
%
%*********************************************************
%加速度计三个轴向的零点电压
%zero_ax=?
%zero_ay=?
%zero_az=?
%加速度计三个轴向的电压/加速度比值
%rate_ax=? %单位是m/s^2/V
%rate_ay=?
%rate_az=?
%acc_x=acc_x*acc_rate;
%acc_y=acc_y*acc_rate;
%acc_z=acc_z*acc_rate;
aver_acc_x=mean(acc_x)
aver_acc_y=mean(acc_y)
aver_acc_z=mean(acc_z)
%采样时间
dtime=0.01;
tm=0:dtime:0.01* (size(gyro_x2)-1);
%个数num
n_point=size(gyro_x2);
%图1
figure
plot(tmdata_acc(1:)‘-‘tmdata_acc(2:)‘.‘tmdata_acc(3:)‘-.‘);
title(‘加速度计的采样曲线‘);
legend(‘x_ACC‘‘Y_ACC‘‘Z_ACC‘);
xlabel(‘Time / (10ms)‘);
ylabel(‘Accelerate/ (m/s‘‘)‘);
grid on;
%plot(tmacc_x‘-‘tmacc_y‘.‘tmacc_z‘-.‘);
%title(‘加速度的计的采样曲线‘):
%对采样曲线进行低通滤波
a=[12421];
%gyro_x=filter(a/sum(a)1gyro_x);
%gyro_y=filter(a/sum(a)1gyro_y);
%gyro_z=filter(a/sum(a)1gyro_z);
%比例变换
gyro_x=gyro_x/1024*3/0.6;
gyro_y=gyro_y/1024*3/0.6;
gyro_z=gyro_z/1024*3/0.6;
%零点电压--陀螺仪,取前80个数的平均电压
zero_gx=sum(gyro_x(1:80))/80
zero_gy=sum(gyro_y(1:80))/80
zero_gz=sum(gyro_z(1:80))/80
%减去零点
gyro_x=(gyro_x-zero_gx)/0.0125/180*pi;
gyro_y=(gyro_y-zero_gy)/0.0125/180*pi;
gyro_z=(gyro_z-zero_gz)/0.0125/180*pi;
%gyro_x=(gyro_x-2.5)/0.0125/180*pi;
%gyro_y=(gyro_y-2.5)/0.0125/180*pi;
%gyro_z=(gyro_z-2.5)/0.0125/180*pi;
%测试数据
accelerate=zeros(3n_point);
accelerate(11:100)=10;
accelerate(1101:200)=-10;
accelerate(1201:300)=0;
%陀螺仪数据
gyro_x=zeros(1n_point);
gyro_y=zeros(1n_point);
gyro_z=zeros(1n_point);
gyro_z(1:100)=pi/3;
gyro_z(101:200)=-pi/3;
%重力轴始终有加速度
accelerate(3:)=accelerate(3:)+9.8;
figure
plot(tmaccelerate(1:)‘-‘tmaccelerate(2:)‘.‘tmaccelerate(3:)‘-.‘);
title(‘加速度计的采样曲线‘);
legend(‘x_ACC‘‘Y_ACC‘‘Z_ACC‘);
xlabel(‘Time / (10ms)‘);
ylabel(‘Accelerate/ (m/s‘‘)‘);
grid on;
%画出陀螺仪的采样曲线
figure
plot(tmgyro_x‘r-‘tmgyro_y‘g.‘tmgyro_z‘b-.‘);
title(‘陀螺仪的采样曲线‘);
legend(‘x_Gyro‘‘Y_Gyro‘‘Z_Gyro‘);
xlabel(‘Time / (10ms)‘);
ylabel(‘Angel_rate/ (degree/s)‘);
grid on;
%size(gyro_x)
%size(gyro_y)
%size(gyro_z)
data_gyro=[gyro_x;gyro_y;gyro_z];
%转移矩阵--即方向余弦矩阵
T=eye(3); %T是3*3的单位矩阵初始转移矩阵
%四元数矩阵,存储每步更新之后的四元数,方便以后绘图
Q=zeros(4n_point);
%四元数的初始值确定,假定一开始导航坐标系与载体坐标系是重合的,因此方向余弦矩阵,是单位矩阵,利用它们之间的关系确定四元数的初始值。
Q(11)=0.5*sqrt(1+T(11)+T(22)+T(33));
Q(21)=0.5*sqrt(1+T(11)-T(22)-T(33));
Q(31)=0.5*sqrt(1-T(11)+T(22)-T(33));
Q(41)=0.5*sqrt(1-T(11)-T(22)+T(33));
%参见捷联惯性导航技术31页3.64式 在旋转90度时不适用
%Q(11)=0.5*sqrt(1+T(11)+T(22)+T(
相关资源
- MATLAB代码卷积运算转化为乘法
- mdp马尔可夫决策过程2009年matlab源码,
- matlab实现newton迭代法
- 标准粒子群算法代码PSO
- matlab处理数字图像贴标签
- MATLAB程序(图像去噪 滤波 锐化 边缘
- MATLAB多径+扩频+rake接受+误码率曲线
- 二维云模型MATLAB
- 用于解决复杂网络社区划分的FastNew
- MIMO 注水算法 Matlab代码
- 分布式电源接入对配电网的影响
- matlab材料力学剪力图弯矩图绘制程序
- 路径规划 matlab
- Matlab图像识别
- 图像滤波Matlab代码
- 微电网光伏并网过程进行仿真microgr
-
双极性SPWM的simuli
nk仿真 - 无网格matlab程序
- 将世界时间转换为格林威治恒星时的
- MATLAB串口采集数据进阶
- MATLABGUI图割法(Grabcut)对象分割
- Matlab光学实验程序
- 人眼识别matlab
- 传染病模型的MATLAB程序和图数学模型
- VREP与MATLAB联合仿真程序--UR5机械臂动
- MATLAB-robotic-toolbox工具箱学习笔记
- 用matlab实现银行卡卡号定位并自动切
- 采用格子bolzmann方法MATLAB模拟圆柱绕流
- matlab 实现语音增强含维纳滤波、谱减
- 费诺编码matlab程序
评论
共有 条评论