基于惯性传感器的室内导航的MATLAB轨迹仿真

基于惯性传感器的室内导航系统，最后可以得到三轴加速度数据，脚速度数据，三维的室内轨迹，位置，是自己学习惯性导航最好的一个仿真程序，值得5分！

classdef AHRS < handle    %classdel是定义类名，后面的<是表示继承，Matlab允许多重继承，继承自handle类

    %% Public properties
    properties （Access = public）
        SamplePeriod = 1/256;
        Quaternion = [1 0 0 0];     % output quaternion describing the sensor relative to the Earth 输出描述传感器相对于地球的四元数
        Kp = 2;                     % proportional gain 比例增益
        Ki = 0;                     % integral gain 积分增益
        KpInit = 200;               % proportional gain used during initialisation 初始化期间使用的比例增益
        InitPeriod = 5;             % initialisation period in seconds 初始化时间（秒）
    end

    %% Private properties%特性
    properties （Access = private）
        q = [1 0 0 0];              % internal quaternion describing the Earth relative to the sensor 描述地球相对于传感器的内部四元数
        IntError = [0 0 0]‘;        % integral error 积分误差
        KpRamped;                   % internal proportional gain used to ramp during initialisation 初始化期间用于斜坡的内部比例增益
    end

    %% Public methods
    methods （Access = public）
        function obj = AHRS（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};
                    obj.q = quaternConj（obj.Quaternion）;
                elseif  strcmp（varargin{i} ‘Kp‘） obj.Kp = varargin{i+1};
                elseif  strcmp（varargin{i} ‘Ki‘） obj.Ki = varargin{i+1};
                elseif  strcmp（varargin{i} ‘KpInit‘） obj.KpInit = varargin{i+1};
                elseif  strcmp（varargin{i} ‘InitPeriod‘） obj.InitPeriod = varargin{i+1};                    
                else error（‘Invalid argument‘）;
                end
                obj.KpRamped = obj.KpInit;
            end;
        end
        function obj = Update（obj Gyroscope Accelerometer Magnetometer）
            error（‘This method is unimplemented‘）;
        end
        function obj = UpdateIMU（obj Gyroscope Accelerometer）

            % Normalise accelerometer measurement
            if（norm（Accelerometer） == 0）                                          	% handle NaN
                warning（0 ‘Accelerometer magnitude is zero.  Algorithm update aborted.‘）;
                return;
            else
                Accelerometer = Accelerometer / norm（Accelerometer）;                % normalise measurement
            end

            % Compute error between estimated and measured direction of
            % gravity 估计和测量重力方向之间的计算误差
            v = [2*（obj.q（2）*obj.q（4） - obj.q（1）*obj.q（3））
                2*（obj.q（1）*obj.q（2） + obj.q（3）*obj.q（4））
                obj.q（1）^2 - obj.q（2）^2 - obj.q（3）^2 + obj.q（4）^2];               	% estimated direction of gravity估计重力方向
            error = cross（v Accelerometer‘）;  %C = cross（AB）返回向量叉积A和B即C = A x B
            
%            	% Compute ramped Kp value used during init period 计算在初始化期间使用的Kp值
%             if（obj.KpRamped > obj.Kp）
%      

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    .......      2039  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\.gitignore

     文件       5731  2018-03-15 09:59  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\AHRS.m

    .......    391195  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Datasets\spiralStairs.BIN

    .......   1200557  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Datasets\spiralStairs_CalInertialAndMag.csv

    .......    441717  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Datasets\stairsAndCorridor.BIN

    .......   1363725  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Datasets\stairsAndCorridor_CalInertialAndMag.csv

    .......    290660  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Datasets\straightLine.BIN

    .......    905489  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Datasets\straightLine_CalInertialAndMag.csv

    .......       202  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\axisAngle2quatern.m

    .......       502  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\axisAngle2rotMat.m

    .......       508  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\euler2rotMat.m

    .......       604  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\quatern2euler.m

    .......       527  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\quatern2rotMat.m

     文件        117  2017-09-07 11:17  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\quaternConj.m

    .......       337  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\quaternProd.m

    .......       162  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\quaternRotate.m

    .......       354  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\rotMat2euler.m

     文件       1365  2017-09-07 19:03  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\rotMat2quatern.m

    .......      2168  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\Quaternions\Testscript.m

     文件       8367  2018-03-20 08:19  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\script.m

    .......      9275  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\SixDofAnimation.m

    .......      3551  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\ADXL345busDatabaseClass.m

    .......      2032  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\AnalogueInputDatabaseClass.m

    .......      1858  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\BatteryAndThermometerDatabaseClass.m

    .......       702  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\CalADXL345busDataClass.m

    .......       704  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\CalAnalogueInputDataClass.m

    .......       777  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\CalBatteryAndThermometerDataClass.m

    .......       819  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\CalInertialAndMagneticDataClass.m

    .......       477  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\CommandDataClass.m

    .......      1504  2017-08-03 21:31  Gait-Tracking-With-x-IMU-master\Gait Tracking With x-IMU\ximu_matlab_library\DatabaseClass.m

............此处省略29个文件信息