Madgwick解算小结

重力加速度归一化后的向量是[0,0,1]T,转换到机体坐标系后为

q−1×g×q=[2(q1q3−q0q2) , 2(q0q1+q2q3) , 2(12−q21−q22)]T

机体坐标系下测得的重力向量可以表示为[ax,ay,az]T那么误差向量就可以表示为：

E=[2(q1q3−q0q2)−ax , 2(q0q1+q2q3)−ay , 2(12−q21−q22)−az]T

从优化的角度，就可以得到一个关于四元数q的最小二乘问题：

f(q)=E

min12||f(q)||

用梯度下降算法，矢量函数f(q)的梯度为：

J(q)Tf(q)

J(q)=
J(q)Tf(q)为：

于是

qt=qt−1+μJ(q)Tf(q)||J(q)Tf(q)||

这里的μ表示可以认为是梯度下降的收敛速度。
接下来就是把与陀螺仪测出来的数据融合，假设由微分方程求解的姿态四元数为 qw ,加速计测出来的四元数为 qa，有：q=α.qa+(1−α).qw
由于陀螺仪数据是发散的，所以α的最佳值就是让陀螺仪解出来的四元数发散速度和加速计解出来的收敛速度相等。有：

这里的β为陀螺仪的飘移速度，解出α的值为：d当β和Δt都接近0的时候，有，带入q=α.qa+(1−α).qw，
有
这里的

fg=J(q)Tf(q)

考虑磁力计，原理一样，只是现在的误差向量变成了六纬：对应的雅各比矩阵和梯度算子都要加磁力计部分
单独对磁力计部分误差求雅各比矩阵：

于是新的雅各比矩阵就是：，再遇陀螺仪读数进行融合,将fg换成新的雅各比转置与误差函数即可。
关于这里的β，代码里的求法为beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0))，主要就是通过gyro的芯片手册查找到gyroscopeMeasurementError具体原理不明