classdef MahonyAHRS < handle
%MAYHONYAHRS Madgwick's implementation of Mayhony's AHRS algorithm
%
% For more information see:
% http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
%
% Date Author Notes
% 28/09/2011 SOH Madgwick Initial release
%% Public properties
properties (Access = public)
SamplePeriod = 1/256;
Quaternion = [1 0 0 0]; % output quaternion describing the Earth relative to the sensor
Kp = 1; % algorithm proportional gain
Ki = 0; % algorithm integral gain
end
%% Public properties
properties (Access = private)
eInt = [0 0 0]; % integral error
end
%% Public methods
methods (Access = public)
function obj = MahonyAHRS(varargin)
for i = 1:2:nargin
if strcmp(varargin{i}, 'SamplePeriod'), obj.SamplePeriod = varargin{i+1};
elseif strcmp(varargin{i}, 'Quaternion'), obj.Quaternion = varargin{i+1};
elseif strcmp(varargin{i}, 'Kp'), obj.Kp = varargin{i+1};
elseif strcmp(varargin{i}, 'Ki'), obj.Ki = varargin{i+1};
else error('Invalid argument');
end
end;
end
function obj = Update(obj, Gyroscope, Accelerometer, Magnetometer)
q = obj.Quaternion; % short name local variable for readability
% Normalise accelerometer measurement
if(norm(Accelerometer) == 0), return; end % handle NaN
Accelerometer = Accelerometer / norm(Accelerometer); % normalise magnitude
% Normalise magnetometer measurement
if(norm(Magnetometer) == 0), return; end % handle NaN
Magnetometer = Magnetometer / norm(Magnetometer); % normalise magnitude
% Reference direction of Earth's magnetic feild
h = quaternProd(q, quaternProd([0 Magnetometer], quaternConj(q)));
b = [0 norm([h(2) h(3)]) 0 h(4)];
% Estimated direction of gravity and magnetic field
v = [2*(q(2)*q(4) - q(1)*q(3))
2*(q(1)*q(2) + q(3)*q(4))
q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
w = [2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))
2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))
2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)];
% Error is sum of cross product between estimated direction and measured direction of fields
e = cross(Accelerometer, v) + cross(Magnetometer, w);
if(obj.Ki > 0)
obj.eInt = obj.eInt + e * obj.SamplePeriod;
else
obj.eInt = [0 0 0];
end
% Apply feedback terms
Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;
% Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
% Integrate to yield quaternion
q = q + qDot * obj.SamplePeriod;
obj.Quaternion = q / norm(q); % normalise quaternion
end
function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
q = obj.Quaternion; % short name local variable for readability
% Normalise accelerometer measurement
if(norm(Accelerometer) == 0), return; end % handle NaN
Accelerometer = Accelerometer / norm(Accelerometer); % normalise magnitude
% Estimated direction of gravity and magnetic flux
v = [2*(q(2)*q(4) - q(1)*q(3))
2*(q(1)*q(2) + q(3)*q(4))
q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
% Error is sum of cross product between estimated direction and measured direction of field
e = cross(Accelerometer, v);
if(obj.Ki > 0)
obj.eInt = obj.eInt + e * obj.SamplePeriod;
else
obj.eInt = [0 0 0];
end
% Apply feedback terms
Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;
% Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
% Integrate to yield quaternion
q = q + qDot * obj.SamplePeriod;
obj.Quaternion = q / norm(q); % normalise quaternion
end
end
end
没有合适的资源?快使用搜索试试~ 我知道了~
基于MATLAB实现的SOH关于IMU进行姿态求解的C代码,重力约束法求解和梯度下降法求解+使用说明文档
共16个文件
m:13个
mat:2个
md:1个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
0 下载量 115 浏览量
2024-05-22
17:17:42
上传
评论
收藏 262KB RAR 举报
温馨提示
CSDN IT狂飙上传的代码均可运行,功能ok的情况下才上传的,直接替换数据即可使用,小白也能轻松上手 【资源说明】 基于MATLAB实现的SOH关于IMU进行姿态求解的C代码,里面包含了两套代码,分别是重力约束法求解和梯度下降法求解+使用说明文档 1、代码压缩包内容 主函数:main.m; 调用函数:其他m文件;无需运行 运行结果效果图; 2、代码运行版本 Matlab 2020b;若运行有误,根据提示GPT修改;若不会,私信博主(问题描述要详细); 3、运行操作步骤 步骤一:将所有文件放到Matlab的当前文件夹中; 步骤二:双击打开main.m文件; 步骤三:点击运行,等程序运行完得到结果; 4、仿真咨询 如需其他服务,可后台私信博主; 4.1 期刊或参考文献复现 4.2 Matlab程序定制 4.3 科研合作 功率谱估计: 故障诊断分析: 雷达通信:雷达LFM、MIMO、成像、定位、干扰、检测、信号分析、脉冲压缩 滤波估计:SOC估计 目标定位:WSN定位、滤波跟踪、目标定位 生物电信号:肌电信号EMG、脑电信号EEG、心电信号ECG 通信系统:DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测识别融合、LEACH协议、信号检测、水声通信 5、欢迎下载,沟通交流,互相学习,共同进步!
资源推荐
资源详情
资源评论
收起资源包目录
基于MATLAB实现的SOH关于IMU进行姿态求解的C代码,里面包含了两套代码,分别是重力约束法求解和梯度下降法求解+使用说明文档.rar (16个子文件)
madgwick_algorithm_matlab
quaternion_library
axisAngle2quatern.m 650B
rotMat2euler.m 645B
euler2rotMat.m 938B
quaternConj.m 378B
quatern2euler.m 878B
rotMat2quatern.m 1KB
quaternProd.m 661B
axisAngle2rotMat.m 965B
quatern2rotMat.m 825B
TestScript.m 2KB
ExampleData.mat 99KB
euler.mat 155KB
@MahonyAHRS
MahonyAHRS.m 5KB
ExampleScript.m 3KB
@MadgwickAHRS
MadgwickAHRS.m 5KB
使用说明文档.md 13KB
共 16 条
- 1
资源评论
IT狂飙
- 粉丝: 4779
- 资源: 2658
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功