姿态解算

一阶龙格-库塔法（First-order Runge-Kutta）公式：

新的 $q_0$ = 旧的 $q_0$ + $\frac{1}{2} \times (-q_1 \Delta\theta_x - q_2 \Delta\theta_y - q_3 \Delta\theta_z)$
新的 $q_1$ = 旧的 $q_1$ + $\frac{1}{2} \times (q_0 \Delta\theta_x + q_2 \Delta\theta_z - q_3 \Delta\theta_y)$
新的 $q_2$ = 旧的 $q_2$ + $\frac{1}{2} \times (q_0 \Delta\theta_y - q_1 \Delta\theta_z + q_3 \Delta\theta_x)$
新的 $q_3$ = 旧的 $q_3$ + $\frac{1}{2} \times (q_0 \Delta\theta_z + q_1 \Delta\theta_y - q_2 \Delta\theta_x)$

龙格库塔公式在结合ICM42688中的化简

由于这四个公式都具有相似性，所以只分析$q_0$的公式。
首先角速度变化量$\Delta \theta=\omega\times \Delta t$.对于ICM42688和绝大多数陀螺仪来说，角速度$\omega$等于$\frac{\text{寄存器值} }{\text{ssf} }$,ssf是LSB的精度，单位为LSB/(°/s)，即ssf的值刚好是1°/s。ICM42688的ssf最大能取到2097.2。另外，需要把角度转化为弧度值，即$\omega=\frac{reg*\pi}{180*ssf}$；

时间变化量$\Delta t$意思是 这一次更新和上一次更新的时间间隔。当然，这只是理想的定义，即假设传感器的ADC无限快，数据寄存器内的数据是连续变化的，那么此时我们的算法就可以通过测量此次更新和上次更新的时间间隔来定义这个$\Delta t$。但是实际情况是传感器的数据更新是很慢的，对于ICM42688来说，可以通过配置其ODR位来控制更新频率，最高有32kHz（这其实已经很快了）。但是假设我们的计算频率远远高于32kHz，那么很多时候两次计算之间的角速度都没有变化，姿态更新没有任何意义；而若计算频率远远低于32k，此种情况确实可以正常解算，但是没有好好利用32kHz的更新频率，错过了很多信息。为了解决这种情况，可以使用ICM42688内自带的FIFO，可以严格按照ODR定义的更新频率把数据塞入FIFO中。这样，我们的解算就可以对每一次更新的数据都进行解算。这种情况下带来的另一个巨大好处是，可以把$\Delta t$直接定义为$\frac{1}{ODR}$(其实可以感觉到$\Delta t$最准确的定义是这次更新和上次更新所使用的原数据的获取时间差)。

带入公式中，可得
新的 $q_0$ = 旧的 $q_0$ + $\frac{\pi}{2180ssf*ODR} \times (-q_1 \mathrm{reg}_x - q_2 \mathrm{reg}_y - q_3 \mathrm{reg}_z)$
根据这个就可以写出该公式的函数了：

传入的ang直接是角速度值（而不是寄存器值）：

static inline void Runge_Kutta(float *q, float *vel, uint16_t freq)

{

float norm = 0.0f;

float coe = 114.59f * freq;

float q_old[4] = {q[0], q[1], q[2], q[3]};

  

q[0] += (-q_old[1]*vel[0]-q_old[2]*vel[1]-q_old[3]*vel[2]) / coe;

  

q[1] += ( q_old[0]*vel[0]+q_old[2]*vel[2]-q_old[3]*vel[1]) / coe;

  

q[2] += ( q_old[0]*vel[1]-q_old[1]*vel[2]+q_old[3]*vel[0]) / coe;

  

q[3] += ( q_old[0]*vel[2]+q_old[1]*vel[1]-q_old[2]*vel[0]) / coe;

  

norm = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);

  

q[0] /= norm;

q[1] /= norm;

q[2] /= norm;

q[3] /= norm;

}