【3DoF算法】

VR 3DoF算法介绍

核心：3DoF算法应用场景，在VIO应用中，当只有测量没有观测的情况下，6DoF算法的预测会退化成一个只有测量的3DoF算法，这时候需要使用3DoF算法，来更加稳定准确的获取3DoF位姿，直到6DoF算法重新找回。

算法	计算方式	计算特点	输入	输出	精度	使用场景	计算速度
Madgwick算法	基于四元数的互补滤波	无需矩阵运算，计算速度快	三轴角速度和三轴加速度	四元数和欧拉角	较高	适用于姿态估计	较快
Mahony算法	基于四元数的互补滤波	无需矩阵运算，计算速度快	三轴角速度和三轴加速度	四元数和欧拉角	较高	适用于姿态估计	较快
Extended Kalman Filter (EKF)	基于卡尔曼滤波的状态估计	能够处理非线性系统	三轴角速度和三轴加速度	四元数和欧拉角	高	适用于姿态估计和位置估计	较慢
Complementary Filter	基于加权平均的滤波	简单易实现，计算速度快	三轴角速度和三轴加速度	四元数和欧拉角	低	适用于姿态估计	较快

需要注意的是，以上表格中的精度和计算速度是相对而言的，具体的数值会受到多种因素的影响，如传感器精度、采样率、算法参数等。因此，在实际应用中，需要根据具体情况选择合适的算法，并进行适当的参数调整和优化。

0. VR 3DoF算法特性小结

VR 3DoF姿态解算大多采用磁强计和IMU传感器实现，IMU传感器内部包含三轴陀螺仪、三轴加速度计，常用的姿态解算算法包含三种：

互补滤波算法；
梯度下降算法；
Kalman滤波算法。

1.互补滤波算法

互补滤波算法是三种算法中最简单的，算法对加速度计测量姿态进行低通滤波，对陀螺仪测量的角度增量进行高通滤波，并将两个滤波器合成一个，实现互补，得到一个线性估计，其中水平姿态通过加速度计观测实现，航向通过磁强计观测实现。

互补滤波用公式介绍如下：

陀螺积分角度 =陀螺积分角度 + 角速度 * dt；

融合角度 = 陀螺仪权值 * 陀螺积分角度 + (1 - 陀螺仪权值) * 加速度/磁强计角度。

2. madgwick(梯度下降算法)

Madgwick IMU Filter论文
Madgwick算法详细解读

An efficient orientation filter for inertial and inertial/magnetic sensor arrays论文
[代码]
工程特性

梯度下降算法是madgwick在2010提出的，下图来自madgwick论文，其核心是将加速度计和磁力计通过梯度下降法得到的姿态与由陀螺仪积分得到的姿态，进行线性融合，得到最优的姿态。梯度下降算法的优势在于姿态精度较互补滤波算法要高。

3. Kalman滤波算法

Kalman滤波算法需要分析系统特性，建立系统状态方程和观测方程，并根据系统误差特性，设立合适的系统噪声方差阵Q和观测噪声协方差阵R。Kalman滤波算法优点是模型准确，可以实现陀螺零偏估计，缺点是计算量大。

综上，根据精度及计算量需求，进行算法的选择。