代码:

classdef MadgwickAHRS < handle
%MADGWICKAHRS Implementation of Madgwick's IMU and AHRS algorithms
%
% 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
Beta = 1; % algorithm gain
end %% Public methods
methods (Access = public)
function obj = MadgwickAHRS(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}, 'Beta'), obj.Beta = 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)]; % Gradient decent algorithm corrective step
F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)
2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3)) - Magnetometer(1)
2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4)) - Magnetometer(2)
2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2) - Magnetometer(3)];
J = [-2*q(3), 2*q(4), -2*q(1), 2*q(2)
2*q(2), 2*q(1), 2*q(4), 2*q(3)
0, -4*q(2), -4*q(3), 0
-2*b(4)*q(3), 2*b(4)*q(4), -4*b(2)*q(3)-2*b(4)*q(1), -4*b(2)*q(4)+2*b(4)*q(2)
-2*b(2)*q(4)+2*b(4)*q(2), 2*b(2)*q(3)+2*b(4)*q(1), 2*b(2)*q(2)+2*b(4)*q(4), -2*b(2)*q(1)+2*b(4)*q(3)
2*b(2)*q(3), 2*b(2)*q(4)-4*b(4)*q(2), 2*b(2)*q(1)-4*b(4)*q(3), 2*b(2)*q(2)];
step = (J'*F);
step = step / norm(step); % normalise step magnitude % Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step'; % 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 % Gradient decent algorithm corrective step
F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];
J = [-2*q(3), 2*q(4), -2*q(1), 2*q(2)
2*q(2), 2*q(1), 2*q(4), 2*q(3)
0, -4*q(2), -4*q(3), 0 ];
step = (J'*F);
step = step / norm(step); % normalise step magnitude % Compute rate of change of quaternion
qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step'; % Integrate to yield quaternion
q = q + qDot * obj.SamplePeriod;
obj.Quaternion = q / norm(q); % normalise quaternion
end
end
end

  

matlab面向对象设计---类的概念和使用的更多相关文章

  1. day24 Pythonpython 面向对象设计 类

    将一些相同特征和动作的成为类,现有类才能创建对象,对象就是特征和动作的结合体 类:把一类事物的相同特征和动作整合到一起就是类.类是一个抽象概念 对象:就是基于类而创建的一个具的事物(具体存在的),也是 ...

  2. js中的面向对象--类似于类的概念

    创建对象的几种常用方式 1.使用Object或对象字面量创建对象 2.工厂模式创建对象 3.构造函数模式创建对象 4.原型模式创建对象 1.使用Object或对象字面量创建对象 使用object va ...

  3. python函数的面向对象——面向对象设计

    通过几个函数式编号演进,理解面向对象设计 def01.py dog1 = { 'name':'元昊', 'gender':'母', 'type':'藏獒' } dog2 = { 'name':'李李' ...

  4. day24:面向对象设计与面向对象编程、类和对象

    一.三大编程范式: 面向过程: 面向函数: 面向对象: 二.程序的进化论: 1.编程最开始就是无组织无结构,从简单控制流中按步写指令 2.从上述的指令中提取重复的代码块或逻辑,组织到一起(比方说,你定 ...

  5. UML类图与面向对象设计原则

    1. 引言     从大一开始学习编程,到如今也已经有两年了.从最初学习的Html,Js,JaveSe,再到JavaEE,Android,自己也能写一些玩具.学习过程中也无意识的了解了一些所谓的设计模 ...

  6. <九>面向对象分析之UML核心元素之设计类,类,属性,方法,可见性

    设计类

  7. 面向对象:MATLAB的自定义类 [MATLAB]

    https://www.cnblogs.com/gentle-min-601/p/9785812.html 面向对象:MATLAB的自定义类 [MATLAB]   这几天刚刚开始学习MATLAB的面向 ...

  8. C#基础第八天-作业答案-设计类-面向对象方式实现两个帐户之间转账

    using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.T ...

  9. C#基础第八天-作业-设计类-面向对象方式实现两个帐户之间转账

    要求1:完成以下两种账户类型的编码.银行的客户分为两大类:储蓄账户(SavingAccount)和信用账户(CreditAccount),两种的账户类型的区别在于:储蓄账户不允许透支,而信用账户可以透 ...

随机推荐

  1. C# DataSet与DataTable的区别和用法

    DataSet是数据集,DataTable是数据表,DataSet存储多个DataTable.DataSet和DataTable像是专门存储数据的一个容器,在你查询数据库得到一些结果时可以存在里面. ...

  2. Ui大屏

    http://www.uimaker.com/plus/view.php?aid=128661&pageno=1

  3. js-随机生成16进制颜色

    <body onload="color()"></body> <script> function color(){ 方法一: document. ...

  4. SPOJ 1825 Free tour II (树的点分治)

    题目链接 Free tour II 题意:有$N$个顶点的树,节点间有权值, 节点分为黑点和白点. 找一条最长路径使得 路径上黑点数量不超过K个 这是树的点分治比较基本的题,涉及树上启发式合并……仰望 ...

  5. CentOS6.7源码安装MySQL5.6

    1.源码安装MySQL5.6 # CentOS6操作系统安装完成后,默认会在/etc目录下存在一个my.cnf, # 强制卸载了mysql-libs之后,my.cnf就会消失 rpm -qa | gr ...

  6. 1007 Maximum Subsequence Sum

    Given a sequence of K integers { N​1​​, N​2​​, ..., N​K​​ }. A continuous subsequence is defined to ...

  7. SQL 列转行与行转列

    假设有张学生成绩表(tb)如下:Name Subject Result张三 语文 74张三 数学 83张三 物理 93李四 语文 74李四 数学 84李四 物理 94*/ -------------- ...

  8. Android获取状态栏和标题栏的高度

    版权声明:本文为博主原创文章,未经博主允许不得转载. 1.获取状态栏高度: decorView是window中的最顶层view,可以从window中获取到decorView,然后decorView有个 ...

  9. scala工具sbt的安装和使用;idea如何创建scala项目

    scala的sbt类似于java的maven mac:brew install sbt linux:yum Install sbt 或者下载二机制包 使用sbt需要想mvn一样搭建公司私服,不然,下载 ...

  10. ShadowMap渲染阴影方法及问题 【转】

    ShadowMap基于的原理:SM算法是一个2-pass绘制算法,第一pass从光源视点绘制场景,生成SM纹理,第2pass从视点视图按常规方法绘制场景 从光源的位置观察场景,这时候我们看不到的地方就 ...