mahony算法学习感悟和总结

Mahony的原始论文[1]

地理坐标系R

传感器测量坐标系(机体坐标系)b
传感器测量坐标系 的原点为惯性传感器的中心，三个坐标轴分别与惯性传感器的三个测量轴重合。
六轴mahony算法
主要公式：

e = cross(Accelerometer, v); （公式1）
eInt = eInt + e * SamplePeriod; （公式2）
Gyroscope = Gyroscope + Kp * e + Ki * eInt; （公式3）

                                                                                          （公式4）

上面公式中Accelerometer，Gyroscope表示1*3的矩阵，分别是加速度和加速度矩阵。
公式1中cross函数是将Accelerometer和v做向量外积，所以e的大小即：

|e| = |Accelerometer| |v|sinθ

Accelerometer和v有表示什么？
首先Accelerometer表示的是传感器输出的三轴加速度值，v指的是在机体坐标系下的理论重力加速度向量。什么叫在机体坐标系下的理论重力加速度向量？
我们都知道当物体水平静止放置时候，物体的三轴加速度在地理坐标系下可以表示为：g = [0 0 1]T ,这是地理坐标系的三轴加速度值，然而Accelerometer是机体坐标系的三轴加速度值，坐标系不同不能直接参与运算。因此需要将v转化为机体坐标系，如何转呢？这就是旋转矩阵的概念，如果想要推导以及理解四元数可以参考文献[3]，我这里直接给出结论：

也就是说想要把地理坐标系下面的g向量，变成机体坐标系下面的向量v，还需要一个左乘一个旋转矩阵，即：

那么这个公式代表什么呢？
因为Accelerometer 和v是单位向量(在程序里面先做了归一化的)，所以e=sinθ ，而sinθ和θ在小于45度的时候，基本一样大，所以e= θ 。因此，整个式子表示的就是传感器输出的加速度向量和理论重力加速度向量的偏差，偏差向量大小是夹角，方向就是两者的法向量。而这个偏差很大程度上是由陀螺仪数据产生的角速度误差引起的，所以e就可以补偿陀螺仪数据的误差，进而解算出较为准确的姿态。

公式2 eInt = eInt + e * SamplePeriod; 表示对e进行积分,结果= eInt
综上，我们得到了传感器加速度的实际数值，也算出了他和理论状态时的偏差，而这个偏差基本上都是由于角速度误差引起的，通过构建PI补偿器来计算角速度补偿值，所以补偿后的角速度向量Gyroscope：
Gyroscope = Gyroscope + Kp * e + Ki * eInt;
其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差。Kp越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计。反之 Kp越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪。而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的Ki。最后将补偿值补偿给角速度测量值，带入四元数微分方程中即可更新当前四元数。

而最后公式就是四元数微分方程，用来得到四元数的，推导参考文献[2]。

九轴mahony算法和六轴的原理一样，在固定的一个区域内地磁场也是有一个固定的理论值，类似加速度一样。因此也是通过旋转矩阵将地理坐标系的理论地磁场向量，旋转成机体坐标系的理论磁场向量。然后与传感器的磁场向量叉乘得到新的偏差向量，和加速度偏差一个加上去，去更新陀螺仪向量。
e = cross(Accelerometer, v) + cross(Magnetometer, w);
Gyroscope = Gyroscope + Kp * e + Ki * eInt;
eInt = eInt + e * SamplePeriod;
最后也是通过四元数微分方程进行迭代得到新的四元数。

六轴matlab代码：

function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
            q = obj.Quaternion; % short name local variable for readab

            % 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

九轴

 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

[1] Mahony R, Hamel T, Pflimlin J M. Nonlinear Complementary Filters on the Special Orthogonal Group [J]. IEEE Transactions on Automatic Control, 2008, 53(5): 1203-1218
[2] Kuipers J B. Quaternions and rotation sequences [M]. Princeton university press Princeton, 1999.

[3] https://link.zhihu.com/?target=https%3A//krasjet.github.io/quaternion/