三轴磁力计解算姿态（四元数）

原理

根据地磁场向量在水平面上的投影来计算载体的偏航角，类似于加速度计解算姿态，不同在于磁场易受干扰，且只能得到偏航角。

方法

假设导航坐标系为东北天，载体坐标系为右前上。

初始载体坐标系和导航坐标系重合，对应的四元数为q=[1,0,0,0]，使用此四元数表示载体在导航坐标系下的旋转。

总体步骤参考三轴加速度计解算姿态（四元数），主要根据磁力计测得的磁场向量在水平方向上的投影与磁北向量的夹角来旋转载体。

当载体水平放置时，磁力计磁场向量在水平方向上的投影很好计算，直接取x,y分量即可。但是当载体不处于水平时，需要将测量的向量先转换到导航坐标系，然后再取其在水平面上的投影，这样磁力计解算可以不受载体姿态影响。

以下是Eigen3的示例代码，先根据加速度计的数据得到载体坐标系到导航坐标系的旋转矩阵，然后再对磁力计的数据进行旋转，将其旋转至导航坐标系，最后取水平面上的投影来计算偏航角。

#include "Eigen/Core"
#include "Eigen/Geometry"
#include <cmath>

// 初始四元数
Eigen::Quaternionf quaternion = Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f);

while(1) {
	// 读取加速度计数据，单位mg
	float acc[3];
	read_acc(acc);
	// 加速度计测量的加速度向量，单位转为g
	Eigen::Vector3f acceleration = Eigen::Vector3f(acc[0] / 1000.0f, acc[1] / 1000.0f, acc[2] / 1000.0f).normalized();
	// 导航坐标系下的标准重力向量
	Eigen::Vector3f gravity = Eigen::Vector3f(0.0f, 0.0f, 1.0f);
	// 测量的加速度从载体系转换到导航系
	acceleration = (quaternion.toRotationMatrix() * acceleration).normalized();
	// 计算旋转轴
	Eigen::Vector3f rotate_vector = acceleration.cross(gravity);
	// 计算旋转角度
	float theta = acos(acceleration.dot(gravity));
	// 得到旋转对应的四元数
	Eigen::Vector3f v = rotate_vector.normalized() * std::sin(theta / 2.0f);
	Eigen::Quaternionf q = Eigen::Quaternionf(std::cos(theta / 2.0f), v.x(), v.y(), v.z()).normalized();
	// 应用旋转
	quaternion = (q * quaternion).normalized();



	// 磁力计数据，任意单位
	float mag[3];
    read_mag(mag);

    // 磁北方向，导航坐标系（东北天）
    Eigen::Vector3f north = Eigen::Vector3f(0, 1, 0);

    // 磁力计测量的磁场向量，载体坐标系（右前上）
    Eigen::Vector3f magnetometer = Eigen::Vector3f(mag[0], mag[1], mag[2]);

    // 转换到导航坐标系
    magnetometer = quaternion.toRotationMatrix() * magnetometer;

    // xy平面投影
    magnetometer.z() = 0;
    magnetometer.normalize();

    // 计算旋转轴
    rotate_vector = magnetometer.cross(north);
    // 计算旋转角度
    theta = acos(magnetometer.dot(north));
    // 获取旋转四元数
    v = rotate_vector.normalized() * std::sin(theta / 2.0f);
    q = Eigen::Quaternionf(std::cos(theta / 2.0f), v.x(), v.y(), v.z()).normalized();
    // 应用旋转
    quaternion = (q * quaternion).normalized();
}