航姿算法研究《VQF：高精度IMU方向估计与零偏估计和磁干扰抑制》

糖方798

已于 2025-04-29 20:42:30 修改

阅读量499

点赞数 9

文章标签：算法开源 matlab

于 2025-04-29 20:21:00 首次发布

本文链接：https://blog.csdn.net/weixin_53470752/article/details/147542317

版权

文章目录

前言

VQF 全称 Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic Disturbance Rejection，中文翻译为高精度IMU方向估计与偏置估计和磁干扰抑制算法，是导航领域的一种航姿算法，该算法的代码完全开源，本文对其作者发表的论文进行了深入分析，并用Matlab对VQF离线算法进行了测试和复现。

笔者已将论文原文、论文翻译、全部开源代码、测试代码和数据集、复现代码等文件一并打包上传，读者可自行下载。

一、论文介绍

1.1 标题

《VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic Disturbance Rejection》

《VQF: 高精度IMU方向估计与偏置估计和磁干扰抑制》
在这里插入图片描述
arXiv:2203.17024v2 [eess.SY] 27 Oct 2022

1.2 摘要

基于mems的惯性测量单元（imu）的小型化使其在越来越多的应用领域得到广泛应用。在惯性运动跟踪中，位置和速度估计、关节角度估计和三维可视化等数据处理步骤都离不开传感器融合的基本任务——方向估计。估计方向上的误差严重影响所有进一步的处理步骤。最近对现有算法的系统比较表明，开箱即用的精度通常很低，需要针对特定应用进行调优才能获得高精度。在目前的工作中，我们提出并广泛评估了一种基于四元数的方向估计算法，该算法基于一种在近惯性坐标系中滤波加速度测量的新方法，包括陀螺仪偏置估计和磁干扰抑制的扩展，以及离线数据处理的变体。与所有现有的工作相比，我们进行了广泛的评估，使用大量公开可用的数据集和八种文献方法进行比较。该方法优于8种文献方法，平均RMSE为2.9°，而文献方法的误差范围为5.3°~ 16.7°。这种改进的精度相对于技术的状态是观察到不仅在平均，但也为每几个不同的运动特性，以及陀螺仪偏差估计。由于评估是在非常多样化的数据集集合中使用单一固定参数进行的，因此我们得出结论，所提出的方法为广泛的运动、传感器硬件和环境条件提供了前所未有的开箱即用性能。这种方向估计精度的提高有望推进基于imu的运动分析领域，并在许多应用中提供性能优势。所提供的开源实现使所提出的方法易于使用。

1.3 创新点

采用了加速度计与陀螺仪融合的一种新方法：对加速度计数据在近惯性系下进行低通滤波。
基于磁力计的航向校正作为模块化解耦步骤进行，消除了磁干扰对水平角的影响，便于同时估计6轴和9轴姿态。
该算法扩展了陀螺仪在静止和运动时的在线零偏估计以及可选的磁干扰抑制功能。
与以往的绝大多数方法相比，可以使用非正式的离线版本，进一步提高了在不需要实时功能的情况下的准确性。
在c++、Python和Matlab中提供了易于使用的开源算法实现。

在这里插入图片描述

1.4 精度评估

与大多数先前的工作相反，论文所提出的方法使用大量公开可用的数据进行了广泛的评估，并与八种现有的IOE算法进行了比较。
结果表明，该方法的定位估计精度比现有方法提高了1.8~5倍。
对于各种各样的运动、速度和受干扰的环境，所提出的方法可以开箱即用，并且不需要特定于应用程序的参数调整。

在这里插入图片描述

二、算法分析

2.1 基本框架

在这里插入图片描述
论文提及算法的基本框架如上图所示，基本流程是先对陀螺仪原始数据进行积分得到陀螺仪三轴姿态解算四元数结果，下一步用此四元数对加速度计原始数据（三维向量）进行旋转，再对旋转后的加速度计数据向量进行倾斜校正得到修正后的姿态四元数，即陀螺仪、加速度计六轴融合姿态解算四元数结果，如果有磁力计数据，再进一步用六轴姿态四元数对磁力计原始数据（三维向量）进行旋转，用旋转后的磁力计数据向量水平方向的数值计算出方位角，将其补偿到六轴姿态四元数，得到陀螺仪、加速度计、磁力计九轴融合姿态解算四元数结果。

整个姿态解算过程中，3轴、6轴、9轴姿态解算均为可单独进行的解耦步骤。

将此框架再加上对旋转后的加速度计数据进行低通滤波的功能，便是论文提到的 BasicVQF算法，即VQF算法的基础版本。

在基础版本的基础上，加入静止检测和磁场干扰抑制功能，便是论文提到的 VQF算法，即VQF算法的完整版本。

2.2 代码细节

陀螺仪积分解算
陀螺仪更新部分包括静止检测和陀螺仪积分解算。下面是陀螺仪积分解算的源代码，原理较为简单。

 			% 陀螺仪预测步骤（积分）
            % 范数乘以采样时间得到旋转角度（三维）
            gyrNorm = norm(gyrNoBias);
            angle = gyrNorm * obj.coeffs.gyrTs;
            if gyrNorm > eps
                c = cos(angle/2);
                s = sin(angle/2);
                % 四元数更新、归一化
                gyrStepQuat = [c, s*gyrNoBias./gyrNorm]; 
                obj.state.gyrQuat = obj.quatMultiply(obj.state.gyrQuat, gyrStepQuat);
                obj.state.gyrQuat = obj.normalize(obj.state.gyrQuat); 
            end

加速度计姿态修正
加速度计更新部分包括静止检测、加速度计数据低通滤波、加速度计姿态修正以及用卡尔曼滤波估计陀螺仪零偏。下面是加速度计低通滤波和姿态修正部分的源代码和原理分析。

            % 将加计测量值转换到近惯性系（利用陀螺仪的四元数旋转），并低通滤波
            accEarth = obj.quatRotate(obj.state.gyrQuat, acc);
            [obj.state.lastAccLp, obj.state.accLpState] = obj.filterVec(accEarth, obj.params.tauAcc, accTs, obj.coeffs.accLpB, obj.coeffs.accLpA, obj.state.accLpState);

            % 再转换到导航系，并归一化处理
            accEarth = obj.quatRotate(obj.state.accQuat, obj.state.lastAccLp);
            accEarth = obj.normalize(accEarth);

            % 倾斜校正
            q_w = sqrt((accEarth(3)+1)/2); % 计算四元数的实部，用于倾斜校正
            if q_w > 1e-6 % 实部为非负正数
                accCorrQuat = [q_w, 0.5*accEarth(2)/q_w, -0.5*accEarth(1)/q_w, 0]; % 构建倾斜校正四元数
            else
                % to avoid numeric issues when acc is close to [0 0 -1], i.e. the correction step is close (<= 0.00011°) to 180°:
                accCorrQuat = [0 1 0 0];
            end
            obj.state.accQuat = obj.quatMultiply(accCorrQuat, obj.state.accQuat);
            obj.state.accQuat = obj.normalize(obj.state.accQuat);

此部分代码是我认为论文算法最重要的部分，其中对旋转后的加速度计数据进行低通滤波对于提升算法精度起到了关键作用，后面数据测试部分有分析到。

加速度计姿态修正的原理是用陀螺积分得到的姿态四元数对加速度计原始数据向量进行旋转，理论上此时加计向量应回到初始位置 [0 0 1]（假设初始位置为水平静止），但由于陀螺漂移，旋转后的加计向量仍有倾斜角度，计算出该角度进行补偿（倾斜校正），实现对陀螺仪的漂移修正。

第一次旋转的本质是消除设备旋转对加速度测量的影响，在倾斜状态下，加计原始数据的水平方向加速度引入了重力分量，垂直方向加速度引入了运动分量，将其通过陀螺仪积分得到的四元数旋转到近惯性系后，重力加速度与Z轴接近对齐（由于陀螺漂移不完全对齐），达到了将高频运动加速度与低频重力加速度解耦的效果，此时再对加速度计数据进行低通滤波要比直接对原始数据滤波的效果更好，这一点论文中也有提到，如下图：
在这里插入图片描述
第二次旋转也就是倾斜校正是通过将当前重力估计方向与理论方向对齐生成一个校正四元数，也就是当前加计向量旋转到 [0 0 1] 的旋转四元数。本质是向量对齐问题，通过四元数旋转最小化两者差异（类似QUEST算法或梯度下降法），论文算法直接通过几何关系计算四元数，这是一种简化的向量对齐方法，只适用于陀螺漂移这样的小角度修正。

磁力计方位修正
磁力计更新部分包括磁干扰检测、方位角修正、磁干扰抑制。下面是磁力计方位修正部分的源代码。

            % 根据当前磁力计测量值计算不一致角度
            obj.state.lastMagDisAngle = atan2(magEarth(1), magEarth(2)) - obj.state.delta;

            % make sure the disagreement angle is in the range [-pi, pi]
            if obj.state.lastMagDisAngle > pi
                obj.state.lastMagDisAngle = obj.state.lastMagDisAngle - 2*pi;
            elseif obj.state.lastMagDisAngle < -pi
                obj.state.lastMagDisAngle = obj.state.lastMagDisAngle + 2*pi;
            end

在此之前，先用加速度计更新后的六轴姿态解算四元数对磁力计数据进行旋转：

			% bring magnetometer measurement into 6D earth frame
            magEarth = obj.quatRotate(obj.getQuat6D(), mag);

在由加计和陀螺确定的姿态（主要是水平姿态）基础上，利用磁力计水平方向的数据计算出方位角，然后反馈到6D结果，由于磁力计易受干扰，直接使用其方位角可能导致瞬时跳变或持续偏差，论文采用了增量式修正方法，通过一阶滤波逐步调整，保证状态连续：

 			% ensure fast initial convergence 确保快速初始收敛
            if obj.states.kMagInit ~= 0.0
                % make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples
                if k < obj.states.kMagInit
                    k = obj.states.kMagInit;
                end

                % iterative expression to calculate 1/N
                obj.states.kMagInit = obj.states.kMagInit/(obj.states.kMagInit+1);

                % disable if t > tauMag
                if obj.states.kMagInit*obj.params.tauMag < magTs
                    obj.states.kMagInit = 0.0;
                end
            end

            obj.states.delta = obj.states.delta + k*obj.states.lastMagDisAngle;
            % make sure delta is in the range [-pi, pi]
            if obj.states.delta > pi
                obj.states.delta = obj.states.delta - 2*pi;
            elseif obj.states.delta < -pi
                obj.states.delta = obj.states.delta + 2*pi;
            end

静止检测
静止检测也是该论文的创新点之一，静止检测的目的是为了估计陀螺仪的零偏和在磁力计更新部分检测到磁场干扰时提供候选数据。静止检测的原理是将陀螺仪和加速度计数据与提前设定的阈值进行比较，如果小于该阈值，将该数据标记为静止数据，如果静止的连续数据的累积时间达到一个时间阈值，则认为载体在这段时间内静止。

下面分别是陀螺仪静止检测部分和加速度计静止检测部分的代码：

			% 陀螺仪静止检测
            if obj.params.restBiasEstEnabled || obj.params.magDistRejectionEnabled
                % 对陀螺仪测量值进行低通滤波
                    % filterVec 函数用于对向量进行滤波
                    % restFilterTau 是静止检测的滤波时间常数
                    % gyrTs 是陀螺仪的采样时间间隔
                    % restGyrLpB 和 restGyrLpA 是低通滤波器的系数
                    % restGyrLpState 是滤波器的状态
                [gyrLp, obj.state.restGyrLpState] = obj.filterVec(gyr, obj.params.restFilterTau, obj.coeffs.gyrTs, ...
                    obj.coeffs.restGyrLpB, obj.coeffs.restGyrLpA, obj.state.restGyrLpState);

                % 计算滤波前后的偏差、三轴偏差的平方和
                deviation = gyr - gyrLp;
                squaredDeviation = dot(deviation, deviation);

                % 将偏差裁剪阈值(2°)转换为弧度
                biasClip = obj.params.biasClip*pi/180;
                % 如果偏差的平方和大于restThGyr这个阈值(2°)的平方,或者滤波后陀螺仪的最大值大于偏差裁剪阈值2°
                if squaredDeviation >= (obj.params.restThGyr*pi/180.0)^2 || max(abs(gyrLp)) > biasClip
                    obj.state.restT = 0.0; % 静止时间重置为0
                    obj.state.restDetected = false; % 静止检测标志为否，即认为这帧陀螺仪数据被检测为运动状态
                end

                % 更新上一次的滤波后的陀螺仪值
                obj.state.restLastGyrLp = gyrLp;
                % 更新上一次的滤波前后偏差平方和(用于静止检测)
                obj.state.restLastSquaredDeviations(1) = squaredDeviation;
            end

            % 去除估计的陀螺仪偏差（在加速度计更新中计算得到）
            gyrNoBias = gyr - obj.state.bias;

            % 加速度计静止检测
            if obj.params.restBiasEstEnabled
                % 对加速度计测量值进行低通滤波
                    % filterVec 函数用于对向量进行滤波
                    % restFilterTau 是静止检测的滤波时间常数
                    % accTs 是加速度计的采样时间间隔
                    % restAccLpB 和 restAccLpA 是低通滤波器的系数
                    % restAccLpState 是滤波器的状态
                [accLp, obj.state.restAccLpState] = obj.filterVec(acc, obj.params.restFilterTau, accTs, ...
                    obj.coeffs.restAccLpB, obj.coeffs.restAccLpA, obj.state.restAccLpState);

                 deviation = acc - accLp;
                 squaredDeviation = dot(deviation, deviation);

                % 如果三轴偏差平方和大于加计静止检测阈值(0.5)的平方，该帧数据认为处于运动状态
                if squaredDeviation >= obj.params.restThAcc^2
                    obj.state.restT = 0.0;
                    obj.state.restDetected = false;
                else
                    obj.state.restT = obj.state.restT + accTs; % 增加1帧的静止时间
                    % 如果累计静止时间>=最小静止时间，则认为imu静止。
                    if obj.state.restT >= obj.params.restMinT
                        obj.state.restDetected = true;
                    end
                end
                obj.state.restLastAccLp = accLp;
                obj.state.restLastSquaredDeviations(2) = squaredDeviation;
            end

在整个静止检测算法中，陀螺仪的作用其实是判断载体是否运动，如果判断为运动，则将累积静止时间清零。加速度计的作用是判断载体是否静止，如果判断为静止，增加累积静止时间。之所以这样设计，应该是出于陀螺仪数据高频特性与加速度计数据低频特性的考虑。

静止检测功能开启后，论文用了卡尔曼滤波来估计陀螺仪的零偏：

 			% 偏差估计
            if obj.params.motionBiasEstEnabled || obj.params.restBiasEstEnabled
                biasClip = obj.params.biasClip*pi/180; % 偏差裁剪阈值
                bias = obj.state.bias; % 获取当前陀螺仪偏差估计

                % get rotation matrix corresponding to accGyrQuat
                % 获取与6D姿态四元数（accGyrQuat）对应的旋转矩阵
                accGyrQuat = obj.getQuat6D();
                R = zeros(1, 9);
                R(1) = 1 - 2*accGyrQuat(3)^2 - 2*accGyrQuat(4)^2; % r11
                R(2) = 2*(accGyrQuat(3)*accGyrQuat(2) - accGyrQuat(1)*accGyrQuat(4)); % r12
                R(3) = 2*(accGyrQuat(1)*accGyrQuat(3) + accGyrQuat(4)*accGyrQuat(2)); % r13
                R(4) = 2*(accGyrQuat(1)*accGyrQuat(4) + accGyrQuat(3)*accGyrQuat(2)); % r21
                R(5) = 1 - 2*accGyrQuat(2)^2 - 2*accGyrQuat(4)^2; % r22
                R(6) = 2*(accGyrQuat(3)*accGyrQuat(4) - accGyrQuat(2)*accGyrQuat(1)); % r23
                R(7) = 2*(accGyrQuat(4)*accGyrQuat(2) - accGyrQuat(1)*accGyrQuat(3)); % r31
                R(8) = 2*(accGyrQuat(1)*accGyrQuat(2) + accGyrQuat(4)*accGyrQuat(3)); % r32
                R(9) = 1 - 2*accGyrQuat(2)^2 - 2*accGyrQuat(3)^2; % r33

                % calculate R*b_hat (only the x and y component, as z is not needed)
                % R与偏差估计的乘积(b_hat)
                biasLp = zeros(1, 2);
                biasLp(1) = R(1)*obj.state.bias(1) + R(2)*bias(2) + R(3)*bias(3);
                biasLp(2) = R(4)*bias(1) + R(5)*bias(2) + R(6)*bias(3);

                % low-pass filter R and R*b_hat 低通滤波
                [R, obj.state.motionBiasEstRLpState] = obj.filterVec(R, obj.params.tauAcc, accTs, ...
                    obj.coeffs.accLpB, obj.coeffs.accLpA, obj.state.motionBiasEstRLpState);
                [biasLp, obj.state.motionBiasEstBiasLpState] = obj.filterVec(biasLp, obj.params.tauAcc, ...
                    accTs, obj.coeffs.accLpB, obj.coeffs.accLpA, obj.state.motionBiasEstBiasLpState);

                % set measurement error and covariance for the respective Kalman filter update
                % 量测矩阵R-陀螺仪姿态，量测噪声协方差矩阵-w
                if obj.state.restDetected && obj.params.restBiasEstEnabled
                    % 如果检测到静止且静止偏差估计启用
                    e = obj.state.restLastGyrLp - obj.state.bias;
                    R = eye(3);
                    w = obj.coeffs.biasRestW*[1 1 1];
                elseif obj.params.motionBiasEstEnabled
                    % 如果运动偏差估计启用
                    e = zeros(1, 3);
                    e(1) = -accEarth(2)/accTs + biasLp(1) - R(1)*bias(1) - R(2)*bias(2) - R(3)*bias(3);
                    e(2) = accEarth(1)/accTs + biasLp(2) - R(4)*bias(1) - R(5)*bias(2) - R(6)*bias(3);
                    e(3) = - R(7)*bias(1) - R(8)*bias(2) - R(9)*bias(3);
                    R = reshape(R, 3, 3)';
                    w = [obj.coeffs.biasMotionW, obj.coeffs.biasMotionW, obj.coeffs.biasVerticalW];
                else
                    e = zeros(1, 3); % needed for codegen
                    w = -1; % disable update
                end

                % Kalman filter update
                % step 1: P = P + V (also increase covariance if there is no measurement update!)
                if obj.state.biasP(1,1) < obj.coeffs.biasP0
                    obj.state.biasP(1,1) = obj.state.biasP(1,1) + obj.coeffs.biasV;
                end
                if obj.state.biasP(2,2) < obj.coeffs.biasP0
                    obj.state.biasP(2,2) = obj.state.biasP(2,2) + obj.coeffs.biasV;
                end
                if obj.state.biasP(3,3) < obj.coeffs.biasP0
                    obj.state.biasP(3,3) = obj.state.biasP(3,3) +obj.coeffs.biasV;
                end
                if w(1) >= 0
                    % clip disagreement to -2..2 °/s
                    % (this also effectively limits the harm done by the first inclination correction step)
                    % 将不一致性裁剪到-2..2，这也有效地限制了第一次倾斜校正步骤造成的影响
                    e = obj.clip(e, -biasClip, biasClip);

                    % step 2: K = P R^T inv(W + R P R^T)
                    %K = (obj.state.biasP * R') * inv(diag(w) + R*obj.state.biasP*R');
                    K = (obj.state.biasP * R') / (diag(w) + R*obj.state.biasP*R');

                    % step 3: bias = bias + K (y - R bias) = bias + K e
                    bias = bias + (K * e')';

                    % step 4: P = P - K R P
                    obj.state.biasP = obj.state.biasP - K*R*obj.state.biasP;

                    % clip bias estimate to -2..2 °/s
                    obj.state.bias = obj.clip(bias, -biasClip, biasClip);
                end
            end

论文所谓的静止检测是一种暴力检测，检测的效果完全取决于设定好的数据阈值与时间阈值，不同传感器的灵敏度和不同数据的测试条件与测试环境都大不相同，这种检测方式的作用十分有限。

磁场干扰抑制
原文对磁场干扰抑制的描述：

我们用一套方法将提出的IOE算法进行扩展，启用磁力计读数的自适应滤波，只在减少临时的磁场扰动的影响。采用的测量由三部分组成：
磁场扰动检测(magnetic disturbancedetection)，磁场扰动抑制(magnetic disturbancerejection)，以及新磁场的接收（new magnetic field acceptance）
。下面简要解释这三部分，并在附录F的Algorithm 3中详细描述了完整算法。
磁场扰动检测使用用户定义或自动确定的参考局部磁场的常数和倾角。磁力计只有当测量值接近参考值至少0.5秒时，才被认为是不受干扰的，否则就受到干扰。当磁场被认为不受干扰时，参考值就会缓慢更新，以跟踪常数和倾角的缓慢变化。
如果检测到磁场扰动，磁场扰动抑制会调整用于航向角校正的一阶滤波器的速度。对于高达60秒的干扰，磁力计更新被完全禁用。对于被干扰的较长时间，会以较低的速度执行更新。
最后，新的磁场接收用于处理环境的突然变化，例如，在将地形从室外改为室内或移动到不同的室内房间后不同的局部磁场。当磁场被认为是被扰动的，但看起来是均匀的（在足够长的一段时间内，IMU不是静止的），当前磁场的范数和倾角字段被用作新的参考。

下面分别是磁场扰动检测、新磁场接受和磁场扰动抑制三个部分的代码：

                % 磁干扰检测
                % 如果当前测量值的范数和倾角与参考值的偏差在允许范围内
                if abs(magNormDip(1) - obj.state.magRefNorm) < obj.params.magNormTh*obj.state.magRefNorm && ...
                        abs(magNormDip(2) - obj.state.magRefDip) < obj.params.magDipTh*pi/180.0
                    obj.state.magUndisturbedT = obj.state.magUndisturbedT + magTs; % 未受干扰的时间
                    if obj.state.magUndisturbedT >= obj.params.magMinUndisturbedTime
                        % 如果未受干扰的时间达到最小未受干扰时间
                        obj.state.magDistDetected = false; % 表明未检测到磁干扰
                        % 更新磁场参考值（范数和倾角）
                        obj.state.magRefNorm = obj.state.magRefNorm + obj.coeffs.kMagRef*(magNormDip(1) - obj.state.magRefNorm);
                        obj.state.magRefDip = obj.state.magRefDip + obj.coeffs.kMagRef*(magNormDip(2) - obj.state.magRefDip);
                    end
                else
                    obj.state.magUndisturbedT = 0.0;
                    obj.state.magDistDetected = true;
                end

                % 新磁场接受
                % 如果当前测量值的范数和倾角与参考值的偏差在允许范围内
                if abs(magNormDip(1) - obj.state.magCandidateNorm) < obj.params.magNormTh*obj.state.magCandidateNorm && ...
                        abs(magNormDip(2) - obj.state.magCandidateDip) < obj.params.magDipTh*pi/180.0
                    % 如果陀螺仪的上一次静止检测值的范数大于最小阈值
                    if norm(obj.state.restLastGyrLp) >= obj.params.magNewMinGyr*pi/180.0
                        % 增加候选时间
                        obj.state.magCandidateT = obj.state.magCandidateT + magTs;
                    end
                    % 更新候选磁场参考值
                    obj.state.magCandidateNorm = obj.state.magCandidateNorm + obj.coeffs.kMagRef*(magNormDip(1) - obj.state.magCandidateNorm);
                    obj.state.magCandidateDip = obj.state.magCandidateDip + obj.coeffs.kMagRef*(magNormDip(2) - obj.state.magCandidateDip);
                    
                    % 如果检测到磁干扰，并且候选时间达到或超过相应阈值
                    if obj.state.magDistDetected &&  (obj.state.magCandidateT >= obj.params.magNewTime || ...
                            (obj.state.magRefNorm == 0.0 && obj.state.magCandidateT >= obj.params.magNewFirstTime))
                        % 将候选参考值设置为新的磁场参考值
                        obj.state.magRefNorm = obj.state.magCandidateNorm;
                        obj.state.magRefDip = obj.state.magCandidateDip;
                        obj.state.magDistDetected = false;
                        obj.state.magUndisturbedT = obj.params.magMinUndisturbedTime;
                    end
                else
                    obj.state.magCandidateT = 0.0;
                    obj.state.magCandidateNorm = magNormDip(1);
                    obj.state.magCandidateDip = magNormDip(2);
                end
            end

            % 如果启用了磁干扰抑制功能
            if obj.params.magDistRejectionEnabled
                % magnetic disturbance rejection
                if obj.state.magDistDetected % 检测到磁干扰
                    % 磁干扰时间较短
                    if obj.state.magRejectT <= obj.params.magMaxRejectionTime
                        % 增加磁干扰拒绝时间，增益设置为0，即弃用磁力计数据
                        obj.state.magRejectT = obj.state.magRejectT + magTs;
                        k = 0;
                    else
                        % 按比例减小增益系数
                        k = k/obj.params.magRejectionFactor;
                    end
                else
                    % 如果未检测到磁干扰，按比例减小磁干扰拒绝时间
                    obj.state.magRejectT = max(obj.state.magRejectT - obj.params.magRejectionFactor*magTs, 0.0);
                end
            end

磁场扰动检测也是通过磁力计数据与设定好的阈值进行对比，与静止检测的固定阈值不同的是，用于判断磁场是否干扰的阈值会不断更新，如果磁场在一定时间内被判断为未受干扰，则用于判断的磁干扰阈值会被更新为这段时间内的磁场数据，如果检测到磁场干扰，磁干扰阈值则被替换为上一次检测到载体静止时间内的磁场数据。

对于磁场扰动抑制功能，是当磁干扰时间较短时，直接弃用磁力计数据，不修正方位角，当磁干扰时间较长时，按比例减小增益系数k，即磁力计方位修正中的权重系数，按比例减小的原因与第3点磁力计方位修正中采用的增量式修正方法的原因相同。

磁场干扰检测与抑制的策略看似有用，但问题在于如何给定初始磁干扰界定阈值，磁场信息本来就难以利用，用这样与静止检测类似的暴力比较的方式来判断磁场是否受到干扰存在很大的局限性，可能只适用于个别测试数据或测试场景。

2.3 数据测试

sassari 数据集 slow_v4.mat（正常测试）

姿态角结果：（第三行的图为参考姿态）
在这里插入图片描述
误差结果：

6轴与9轴方位角偏差：

陀螺仪偏置估计值与不确定性：

静止检测与磁干扰检测结果：

MTI-300 数据集 home21.mat

姿态角结果：
在这里插入图片描述
误差结果：

6轴与9轴方位角偏差：

陀螺仪偏置估计值与不确定性：

静止检测与磁干扰检测结果：

3. sassari 数据集 slow_v4.mat（低通滤波功能测试）

6轴姿态角结果（加入低通滤波）

在这里插入图片描述
6轴姿态角结果（去除低通滤波）

在这里插入图片描述
静止检测结果：

在这里插入图片描述

2.4 结果分析

对于论文所提及的几种公开数据集，如sassari数据集、broad数据集，经过我的测试，可以达到论文所宣称的精度，这一点应该毋庸置疑，毕竟数据集公开、论文的算法代码公开，读者可以任意检验论文测试结果是否准确。

论文提到的数据集有个特点是专用于航姿算法测试，测试条件、测试数据较为简单和稳定，是将imu放在固定转台上，对各个轴进行旋转得到的数据。对于这些数据集，算法设置好的陀螺仪零偏估计阈值也可以较好地估计出陀螺仪的零偏，并且磁力计的数据较为可靠，即使是没有陀螺仪零偏估计、没有磁干扰检测的VQF基础版本，也能非常准确地估计出载体的方位角和水平姿态。
对于实际测试数据，如严恭敏教授在其个人网站分享的MTI-300航姿仪跑车数据，方位角结果完全不对，磁干扰检测结果是磁场数据全部受干扰，明显不对。
从低通滤波功能测试结果可以看出，VQF算法精度的提升，主要得益于其对加速度计数据在近惯性系下进行的低通滤波操作。

三、代码复现

VQF算法本质上是一种对加速度计和陀螺仪采用的基于四元数旋转的互补滤波融合算法，如果有磁力计数据，就在六轴姿态估计结果的基础上，用磁力计数据计算出方位角进行补偿，磁力计数据只提供方位信息，与六轴姿态估计过程完全解耦。

论文宣称即使是没有静止检测和磁干扰抑制功能的VQF基础版本的精度，就已经超过了其他的航姿算法，VQF完整版本则比基础版本略有提升，精度相差不大。在我分析了静止检测和磁干扰抑制功能的算法原理之后，结合对数据的测试结果，也发现完整版本所多出的这两个功能的实用性并不强，作用非常有限。

我认为VQF算法的关键在于加速度计与陀螺仪数据的融合方式，和对近惯性系下加速度数据进行低通滤波的操作。为了进一步分析算法原理，并使分析过程更加简洁明了，我对VQF基础版本进行了算法复现，并进行了数据测试，复现算法简称为VQFB。

复现算法去除了低通滤波、静止检测和磁干扰抑制等功能的所有代码，只保留了基础版本的最核心步骤。

3.1 关键代码

以下是复现算法（VQFB）的完整代码：

classdef VQFB < handle
    properties % 属性，对象拥有的特征和状态，用来存储数据
        params = struct( ... % parameters参数
            'tauMag', 9.0 ... % 磁力计更新的时间常数
            )
        states = struct( ... % states状态
            'gyrQuat', [1 0 0 0], ...
            'accQuat', [1 0 0 0], ...            
            'bias', [0 0 0], ...
            'biasP', NaN(3, 3), ...
            'delta', 0.0, ...
            'kMagInit', 0.5, ...
            'lastMagDisAngle', 0.0 ...
            )
        coeffs = struct( ... % coefficients系数
            'gyrTs', -1.0, ...
            'accTs', -1.0, ...
            'magTs', -1.0, ...
            'kMag', -1.0 ...
            )
    end
    %% 更新算法
    methods % 方法，对象能够执行的操作(函数)，用来改变对象的特征和状态或执行任务
        function obj = VQFB(gyrTs, varargin) % 创建一个对象
            % varargin:可变长度的输入参数列表，可用于提供加计和磁的采样时间，以及自定义的属性结构体            
            obj.coeffs.gyrTs = gyrTs;
            assert(isnumeric(obj.coeffs.gyrTs) && isscalar(obj.coeffs.gyrTs) && obj.coeffs.gyrTs > 0);

            if nargin > 1 && isstruct(varargin{nargin - 1}) % 如果提供了属性结构体
                params = varargin{nargin - 1};
                fields = fieldnames(params); % 提取结构体中的字段名
                for i = 1:length(fields) % 赋值给对象
                    obj.params.(fields{i}) = params.(fields{i});
                end
            end

            if obj.coeffs.accTs < 0
                obj.coeffs.accTs = obj.coeffs.gyrTs;
            end
            if obj.coeffs.magTs < 0
                obj.coeffs.magTs = obj.coeffs.gyrTs;
            end        

            obj.setup();
        end
        function updateGyr(obj, gyr)
            assert(all(size(gyr) == [1, 3]), 'gyr has wrong size');

            gyrNoBias = gyr - obj.states.bias;

            gyrNorm = norm(gyrNoBias);
            angle = gyrNorm*obj.coeffs.gyrTs;
            if gyrNorm > eps % 模值为0不旋转
                c = cos(angle/2);
                s = sin(angle/2);
                gyrStepQuat = [c, s*gyrNoBias./gyrNorm]; % 旋转四元数
                obj.states.gyrQuat = obj.quatMultiply(obj.states.gyrQuat, gyrStepQuat); % 旋转
                obj.states.gyrQuat = obj.normalize(obj.states.gyrQuat); % 归一化
            end
        end
        function updateAcc(obj, acc)
            assert(all(size(acc) == [1, 3]), 'gyr has wrong size');
            % accTs = obj.coeffs.accTs;
            %    坐标系转换，将加计向量旋转到导航系Z轴方向（加计本体坐标系→地球东北天坐标系→导航坐标系）
            %将加计向量用陀螺积分得到的四元数旋转，理论上此时加计应旋转回到初始位置[0 0 1]（假设初始位置为水平静止）
            %但由于陀螺漂移，旋转后的加计向量仍有倾斜角度，计算出该角度进行补偿，实现对陀螺仪的漂移修正。
            %    在倾斜状态下，加计原始数据的水平方向加速度引入了重力分量，垂直方向加速度引入了运动分量，
            %将其通过用陀螺积分得到的四元数旋转到近惯性系后，重力加速度与Z轴接近对齐（由于陀螺的漂移不完全对齐）
            %    第一次旋转的本质是消除设备旋转对加速度测量的影响。
            %同时也可达到将高频运动加速度与低频重力加速度解耦的效果，此时再进行低通滤波比一开始就低通滤波效果更好。

            %    滤波的作用：若直接使用未滤波的加速度数据（含动态加速度），校正四元数会受到瞬时运动的影响，导致姿态估计
            %抖动或漂移。

            %    校正四元数的生成是通过将当前重力估计方向与理论方向对齐。本质是向量对齐问题，可通过四元数旋转最小化两者差异
            %（类似QUEST算法或梯度下降法），本代码直接通过几何关系计算四元数，这是一种简化的向量对齐方法，适用于小角度修正。
            accEarth = obj.quatRotate(obj.states.gyrQuat, acc);
            accNavig = obj.quatRotate(obj.states.accQuat, accEarth);
            accNavig = obj.normalize(accNavig);
            % 倾斜校正，校正四元数：当前加计向量旋转到[0 0 1]的旋转四元数
            q_w = sqrt((accNavig(3) + 1)/2);
            if q_w > 1e-6 % 实部为非负正数
                accCorrQuat = [q_w, 0.5*accNavig(2)/q_w, -0.5*accNavig(1)/q_w, 0];
            else 
                accCorrQuat = [0 1 0 0];
            end
            obj.states.accQuat = obj.quatMultiply(accCorrQuat, obj.states.accQuat);
            obj.states.accQuat = obj.normalize(obj.states.accQuat);
        end
        function updateMag(obj, mag)
            assert(all(size(mag) == [1, 3]), 'mag hsa wrong size');
            magTs = obj.coeffs.magTs;

            magEarth = obj.quatRotate(obj.getQuat6D(), mag);
            obj.states.lastMagDisAngle = atan2(magEarth(1), magEarth(2)) - obj.states.delta;
            % make sure the disagreement angle is in the range [-pi, pi]
            if obj.states.lastMagDisAngle > pi
                obj.states.lastMagDisAngle = obj.states.lastMagDisAngle - 2*pi;
            elseif obj.states.lastMagDisAngle < -pi
                obj.states.lastMagDisAngle = obj.states.lastMagDisAngle + 2*pi;
            end

            k = obj.coeffs.kMag;

             % ensure fast initial convergence 确保快速初始收敛
            if obj.states.kMagInit ~= 0.0
                % make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples
                if k < obj.states.kMagInit
                    k = obj.states.kMagInit;
                end

                % iterative expression to calculate 1/N
                obj.states.kMagInit = obj.states.kMagInit/(obj.states.kMagInit+1);

                % disable if t > tauMag
                if obj.states.kMagInit*obj.params.tauMag < magTs
                    obj.states.kMagInit = 0.0;
                end
            end

            obj.states.delta = obj.states.delta + k*obj.states.lastMagDisAngle;
            % make sure delta is in the range [-pi, pi]
            if obj.states.delta > pi
                obj.states.delta = obj.states.delta - 2*pi;
            elseif obj.states.delta < -pi
                obj.states.delta = obj.states.delta + 2*pi;
            end
        end
        function update(obj, gyr, acc, mag)
            obj.updateGyr(gyr);
            obj.updateAcc(acc);
            if nargin >= 4
                obj.updateMag(mag);
            end
        end
        function out = updateBatch(obj, gyr, acc, mag)
            N = size(gyr, 1);
            out6D = zeros(N, 4);
            out3D = zeros(N, 4);

            if nargin >=4
                out9D = zeros(N, 4);
                outDelta = zeros(N, 1);
                for i = 1:N
                    obj.update(gyr(i, :), acc(i, :), mag(i, :));
                    out3D(i, :) = obj.getQuat3D();
                    out6D(i, :) = obj.getQuat6D();
                    out9D(i, :) = obj.getQuat9D();
                    outDelta(i) = obj.getDelta();
                end
                out = struct('quat3D', out3D, 'quat6D', out6D, 'quat9D', out9D, 'delta', outDelta);
            else
                for i = 1:N
                    obj.update(gyr(i, :), acc(i, :));
                    out3D(i, :) = obj.getQuat3D();
                    out6D(i, :) = obj.getQuat6D();
                end
                out = struct('quat3D', out3D, 'quat6D', out6D);
            end
        end
        function out = getQuat3D(obj)
            out = obj.states.gyrQuat;
        end
        function out = getQuat6D(obj)
            out = obj.quatMultiply(obj.states.accQuat, obj.states.gyrQuat);
        end
        function out = getQuat9D(obj)
            out = obj.quatMultiply(obj.states.accQuat, obj.states.gyrQuat);
            out = obj.quatApplyDelta(out, obj.states.delta);
        end
        function out = getDelta(obj)
            out = obj.states.delta;
        end

    end
%% 内部函数
    methods(Access = protected)
        function setup(obj)
            assert(obj.coeffs.gyrTs > 0);
            
            obj.coeffs.kMag = obj.gainFromTau(obj.params.tauMag, obj.coeffs.magTs);

            % obj.resetStates(); % 重置对象的状态(防止需要多次使用同一个对象进行不同的滤波任务等）
        end

        function resetStates(obj)
            obj.states.gyrQuat = [1 0 0 0];
            obj.states.accQuat = [1 0 0 0];
            obj.states.bias = [0 0 0];
            obj.states.delta = 0.0;
        end
    end
    %%  内部封装函数
    methods(Static)
         function k = gainFromTau(tau, Ts)
            assert(Ts > 0, 'Ts must be positive');
            if tau < 0.0
                k = 0;
            elseif tau == 0.0
                k = 1;
            else 
                k = 1 - exp(-Ts/tau);
            end
         end
    end
    %%  四元数和向量运算
    methods(Static)
        function out = quatMultiply(q1, q2)
            w = q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
            x = q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
            y = q1(1) * q2(3) - q1(2) * q2(4) + q1(3) * q2(1) + q1(4) * q2(2);
            z = q1(1) * q2(4) + q1(2) * q2(3) - q1(3) * q2(2) + q1(4) * q2(1);
            out = [w x y z]; 
        end
        function out = quatConj(q)
            out = [q(1) -q(2) -q(3) -q(4)];
        end
        function out = quatApplyDelta(q, delta)
            c = cos(delta/2);
            s = sin(delta/2);
            w = c*q(1) - s*q(4);
            x = c*q(2) - s*q(3);
            y = c*q(3) + s*q(2);
            z = c*q(4) + s*q(1);
            out = [w x y z];
        end
        % 四元数旋转公式：v1 = q × v × q*(共轭)
        function out = quatRotate(q, v)
            x = (1 - 2*q(3)*q(3) - 2*q(4)*q(4))*v(1) + 2*v(2)*(q(3)*q(2) - q(1)*q(4)) + 2*v(3)*(q(1)*q(3) + q(4)*q(2));
            y = 2*v(1)*(q(1)*q(4) + q(3)*q(2)) + v(2)*(1 - 2*q(2)*q(2) - 2*q(4)*q(4)) + 2*v(3)*(q(3)*q(4) - q(2)*q(1));
            z = 2*v(1)*(q(4)*q(2) - q(1)*q(3)) + 2*v(2)*(q(1)*q(2) + q(4)*q(3)) + v(3)*(1 - 2*q(2)*q(2) - 2*q(3)*q(3));
        	out = [x y z];
        end
        function vec = normalize(vec)
            n = norm(vec);
            if n ~= 0.0
                vec = vec/n;
            end
        end
        function  [roll, pitch, yaw] = quat2euler_90(q)
            w = q(1);
            x = q(2);
            y = q(3);
            z = q(4);
            roll = atan2(2*(w*x + y*z), 1 - 2*(x^2 + y^2));
            sinp = 2*(w*y - z*x);
            if abs(sinp) >= 1
                pitch = rad2deg(copysign(pi/2, sinp));
            else
                pitch = rad2deg(asin(sinp));
            end 
            yaw  = rad2deg(atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))) - 90;
            if yaw <= -179.9
                yaw = yaw + 360;
            end
        end
        function  [roll, pitch, yaw] = quat2euler(q)
            w = q(1);
            x = q(2);
            y = q(3);
            z = q(4);
            roll = atan2(2*(w*x + y*z), 1 - 2*(x^2 + y^2));
            sinp = 2*(w*y - z*x);
            if abs(sinp) >= 1
                pitch = rad2deg(copysign(pi/2, sinp));
            else
                pitch = rad2deg(asin(sinp));
            end 
            yaw  = rad2deg(atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2)));
        end
        
    end
end

3.2 数据测试

sassari 数据集 slow_v4.mat

姿态角结果：
在这里插入图片描述
误差结果：（yaw-f 为滤除了360°附近值之后的方位角结果）

6轴与9轴方位角偏差：

MTI-300 数据集 home21.mat

姿态角结果：
在这里插入图片描述

误差结果：
在这里插入图片描述

6轴与9轴方位角偏差：
在这里插入图片描述
3. VQFB与BasicVQF比对

在这里插入图片描述

3.3 结果分析

复现算法（VQFB）相当于在VQF算法基础版本（BasicVQF）的基础上，再去掉对加速度计低通滤波的操作，从对sassari数据集和MTI-300测试数据测试中VQFB与BasicVQF的结果对比中发现，少了低通滤波后，VQFB要比BasicVQF的精度低不少，印证了VQF算法精度的提升主要来源于对加速度计在近惯性系下低通滤波的操作的观点。

同时从测试结果可以看出，6轴与9轴姿态的水平角结果完全相同，说明算法实现了磁力计解耦，磁力计数据只用于修正方位角，而对水平角的估计不产生任何影响。