mpu6050姿态解算与卡尔曼滤波（4）姿态解算

一、状态方程
状态变量选取为姿态四元数$X=[q_0,q_1,q_2,q_3{]}^T$。则根据四元数微分方程
$$\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\omega }_{nbx}^b& -{\omega }_{nby}^b& -{\omega }_{nbz}^b\\ {\omega }_{nbx}^b& 0& {\omega }_{nbz}^b& -{\omega }_{nby}^b\\ {\omega }_{nby}^b& -{\omega }_{nbz}^b& 0& {\omega }_{nbx}^b\\ {\omega }_{nbz}^b& {\omega }_{nby}^b& -{\omega }_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$
取一阶近似
$${\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_k=\left(I+\frac{\Delta t}{2}{\left[\begin{array}{cccc}0& -{\omega }_{nbx}^b& -{\omega }_{nby}^b& -{\omega }_{nbz}^b\\ {\omega }_{nbx}^b& 0& {\omega }_{nbz}^b& -{\omega }_{nby}^b\\ {\omega }_{nby}^b& -{\omega }_{nbz}^b& 0& {\omega }_{nbx}^b\\ {\omega }_{nbz}^b& {\omega }_{nby}^b& -{\omega }_{nbx}^b& 0\end{array}\right]}_{k-1}\right){\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_{k-1}$$
所以状态转移矩阵为
$${\Phi }_{k/k-1}=I+\frac{\Delta t}{2}{\left[\begin{array}{cccc}0& -{\omega }_{nbx}^b& -{\omega }_{nby}^b& -{\omega }_{nbz}^b\\ {\omega }_{nbx}^b& 0& {\omega }_{nbz}^b& -{\omega }_{nby}^b\\ {\omega }_{nby}^b& -{\omega }_{nbz}^b& 0& {\omega }_{nbx}^b\\ {\omega }_{nbz}^b& {\omega }_{nby}^b& -{\omega }_{nbx}^b& 0\end{array}\right]}_{k-1}$$
对于系统噪声$W_{k-1}$可以简单考虑为系统噪声直接作用在状态$X$上，也即
$${\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_k=\left(I+\frac{\Delta t}{2}{\left[\begin{array}{cccc}0& -{\omega }_{nbx}^b& -{\omega }_{nby}^b& -{\omega }_{nbz}^b\\ {\omega }_{nbx}^b& 0& {\omega }_{nbz}^b& -{\omega }_{nby}^b\\ {\omega }_{nby}^b& -{\omega }_{nbz}^b& 0& {\omega }_{nbx}^b\\ {\omega }_{nbz}^b& {\omega }_{nby}^b& -{\omega }_{nbx}^b& 0\end{array}\right]}_{k-1}\right){\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_{k-1}+W_{k-1}$$
事实上角速度${\omega }_{nb}^b$的误差$\Delta \omega$构成了系统噪声，有兴趣可以自行推导其噪声输入矩阵${\Gamma }_{k-1}$。
二、测量方程
选取测量值为mpu6050测量得到的加速度$Z_k=a$，在mpu6050自身无运动加速度的情况下其测量值
$$a=C_n^b\ast {\left[\begin{array}{c}00g\end{array}\right]}^T$$
根据四元数与姿态矩阵的转换，得到
$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=g\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$
线性化和离散化
$${\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]}_k=g{\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]}_{k-1}+g\ast \left[\begin{array}{cccc}-2q_2& 2q_3& -2q_0& 2q_1\\ 2q_1& 2q_0& 2q_3& 2q_2\\ 2q_0& -2q_1& -2q_2& 2q_3\end{array}\right]\left(X_k-{\hat{X}}_{k-1}\right)+V_k$$
到这里我们就获得了线性化的状态方程和测量方程了，按照博客(2)http://t.csdnimg.cn/EYpJ7里标准卡尔曼滤波的公式即可进行滤波，即可获得融合陀螺和加速度计的姿态四元数了。
三、$X$和$P$的初始化
通常情况下，卡尔曼滤波器是收敛的，因此滤波器的初值$X$和$P$可以设置的比较随意。但是对于本文设计的这个滤波器，测量值$Z_k$只包含姿态的一部分信息，因此滤波器状态$X$中必定有发散的部分。简单假设当$X\approx {\left[\begin{array}{cccc}1& q_1& q_2& q_3\end{array}\right]}^T$时，$q_1$、$q_2$即对应俯仰和滚转角，而$q_3$对应方位角，在滤波过程中$q_3$是发散的。尽管滤波器估计出的姿态中方位角是发散的，我们仍然可以取初始化时的方位角为0，这样滤波器估计出的方位角就是相对初始时的方位，在短时间内它仍是可信赖的信息。
对于$X$的初始化可以参考严恭敏老师的《捷联惯导算法与组合导航原理》7.5.4节。以下内容均摘抄自该部分。
记姿态阵$C_b^n$的三个行向量为$C_1$，$C_2$，$C_3$，即
$$C_b^n=\left[\begin{array}{c}{C}_1\\ C_2\\ C_3\end{array}\right]$$
根据加速度测量值$a={\left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right]}^T$构造$C_b^n$的一种简便方法如下：
（1）构造$C_3=\left[\begin{array}{ccc}{C}_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right]/|a|$;
（2）如果$\left|C_{31}\right|>0.5$，则构造$C_2^{\prime }=\left[\begin{array}{ccc}{C}_{32}& -C_{31}& 0\end{array}\right]$，否则构造$C_2^{\prime }=\left[\begin{array}{ccc}0& C_{33}& -C_{32}\end{array}\right]$，归一化得到$C_2=C_2^{\prime }/\left|C_2^{\prime }\right|$。
（3）构造$C_1=C_2\times C_3$。
这样就可以得到初始姿态矩阵$C_b^n$，然后可以得到初始姿态四元数$Q$，作为滤波器状态$X$的初值。
对于$P$阵，由于初始方位角被设定为0，它是我们极其确定的一个值，因此$P_{44}$的初值应当被设置为很小，一般的可以取$<1E-7$，$P_{11}$、$P_{22}$、$P_{33}$的初值可以设置为$1E-2$，在滤波过程中会收敛。
四、$Q$和$R$的设置
对于卡尔曼滤波器，系统噪声$Q$和测量噪声$R$至关重要。
首先考虑$R$，由于测量量选取的就是加速度计测量的加速度，因此$R$就是加速度测量误差$\Delta a$的协方差。考虑三轴加速度计为互相独立的测量，因此$R$为对角矩阵。对于mpu6050这样的低精度imu，可以直接取静态情况下各轴加速度计测量值的3倍标准差的平方作为$R$的对角线元素。
对于$Q$阵，陀螺的测量误差构成了系统噪声。同样取静态情况下陀螺测量值的3倍标准差的平方作为陀螺测量的方差阵，再乘上滤波周期$\Delta t$的平方，即可作为$Q$的对角线元素。然后观察滤波器的运行情况调整$Q$。调整原则是如果想使滤波器更多的信赖陀螺测量则减小$Q$，如果想使滤波器更多的信赖加表测量则增大$Q$。