VR 3DoF算法介绍
核心：3DoF算法应用场景，在VIO应用中，当只有测量没有观测的情况下，6DoF算法的预测会退化成一个只有测量的3DoF算法，这时候需要使用3DoF算法，来更加稳定准确的获取3DoF位姿，直到6DoF算法重新找回。
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滤波算法优点是模型准确，可以实现陀螺零偏估计，缺点是计算量大。
综上，根据精度及计算量需求，进行算法的选择。
