卡尔曼（Kalman）滤波（六）--卡尔曼滤波的应用： 四元数卡尔曼滤波（QKF）的C代码实现姿态解算

0 引言

在捷联惯导工程实践^[6]中，我们希望陀螺仪能够非常精确的获取信息，或者说希望陀螺仪能非常准确的地反映观测量（加速度，磁场等）^[6,7]的真实值，但是这个过程或多或少是受到噪声干扰的，导致测量的不准确；为了能够让陀螺仪在状态更新时做到准确，必须对状态变量和观测量进行数据融合和滤波，从而尽最大限度的降低噪声的干扰。

最常用也最有效的方法非卡尔曼滤波莫属，其在处理高斯模型的系统上效果颇佳；随着计算机技术的发展，Kalman滤波的计算要求和复杂性已不再成为其应用中的阻碍，并且越来越受到人们的青睐。卡尔曼滤波是线性系统的最小方差估值器，但在实际应用中，运用四元数表示的状态模型关于状态变量却是非线性的，一种办法是考虑使用扩展的Kalman滤波（EKF），但是其在求解四元数微分方程时需要求一个雅可比矩阵，计算量复杂，对于实际中的高维问题显得不切实际；另一种办法是考虑使用非线性滤波算法，如无迹的卡尔曼滤波^[5]，粒子滤波等，但他们极其复杂和繁琐的计算量，使其难以在工程实践中得到应用；因此利用汉密尔顿算子将四元数状态方程改写成线性形式，并在计算相关协方差矩阵时进行了改进。

另外，在姿态解算的两个更新过程中，将陀螺仪的原始输出角增量积分得到更新所需要的角度这一步骤显得举足轻重，如果能在陀螺的积分过程中降低噪声对原始输出的影响，那么在后续的迭代更新中无疑减小了噪声的逐级扩散和放大，因为角度和位移的积分均是对时间的积分，如果处理不当，很容易造成噪声随着时间的延续而逐步增大，可想而知难以达到实际应用精度。

1.四元数

2.基于三次样条插值的QKF

2.1状态更新

2.2观测更新

3.C代码实现

初始参数设置：

初始四元数为，

下面是C代码：

#define ROU_K       0.001
#define SIGAMA_1    0.5
#define SIGAMA_2    6
#define SIGAMA_3    0.01

float q_k_k[4]={1,0,0,0};
float p_k_k[7][7]={5,0,0,0,0,0,0,
                   0,5,0,0,0,0,0,
                   0,0,5,0,0,0,0,
                   0,0,0,5,0,0,0,
                   0,0,0,0,5,0,0,
                   0,0,0,0,0,5,0,
                   0,0,0,0,0,0,5};
float p_k_k_q[4][4]={5,0,0,0,
                     0,5,0,0,
                     0,0,5,0,
                     0,0,0,5};

float u_k_k[3]={0,0,0};
float exp_theta_x,exp_theta_y,exp_theta_z;



const float I4[4][4]={1,0,0,0,
                      0,1,0,0,
                      0,0,1,0,
                      0,0,0,1};

const float I7[7][7]={1,0,0,0,0,0,0,
                      0,1,0,0,0,0,0,
                      0,0,1,0,0,0,0,
                      0,0,0,1,0,0,0,
                      0,0,0,0,1,0,0,
                      0,0,0,0,0,1,0,
                      0,0,0,0,0,0,1};

float b_k1[3]={0};
float r_k1[3]={0};
float c_k1[3]={0};
float d_k1[3]={0};

void QKF_update2(void)
{
    float X_k_k[4][3];
    float x_k_k[7];
    float x_k1_k[7]={0};
    float x_k1_k1[7]={0};

    float p_k1_k[7][7]={0};
    float p_k1_k_q[4][4];
    float p_k1_k1[7][7]={0};
    float p_k1_k_v[4][4]={0};
    float p_k_w[7][7]={0};

    float q_k1_k[4];
    float q_k1_k1[4]={0};

    float F_K_K[4][4];
    float F_K[4][4];

    float theta_k[3];
    float theta_k_k[3];

    float M_k_k[4][4];
    float M_k1_k[4][4];

    float H_k1[4][4]={0};
    float H_k1_2[4][7]={0};

    float B_k1[4][4]={0};
    
    float S_k1_k[4][4]={0};
    float K_k1[7][4]={0};

    float pusai[7][7]={0};
    
    float temp_77_1[7][7]={0};
    float temp_77_2[7][7]={0};
    float temp_77_3[7][7]={0};
    float temp_77_4[7][7]={0};

    float temp_44_1[4][4]={0};
    float temp_44_2[4][4]={0};
    float temp_44_3[4][4]={0};
    float temp_44_4[4][4]={0};
 
    float temp_74[7][4]={0};
    float temp_74_2[7][4]={0};
   
    float temp_47_1[4][7]={0};

    float tr;
    float temp; 
      
    unsigned char i=0,j=0;

    for(i=0;i<3;i++)
    {
        b_k1[i]=gyro[i];
    }


    /*下面为时间更新*/
    for(i=0;i<4;i++)
    {
        x_k_k[i]=q_k_k[i];
    }
    for(i=0;i<3;i++)
    {
        x_k_k[i+4]=u_k_k[i];
    }                             //对x_k_k赋值

    for(i=0;i<3;i++)
    {
        theta_k[i]=theta[i];
    }                       //对theta_k赋值

    for(i=0;i<3;i++)
    {
        theta_k_k[i]=theta_k[i]-TWO_TIM*u_k_k[i];
    }                                   //对theta_k_k赋值

    exp_theta_x=exp(0.5*theta_k_k[0]);
    exp_theta_y=exp(0.5*theta_k_k[1]);
    exp_theta_z=exp(0.5*theta_k_k[2]);
    F_K_K[0][0]=F_K_K[1][1]=F_K_K[2][2]=F_K_K[3][3]=1;
    F_K_K[0][1]=F_K_K[3][2]=1/exp_theta_x;
    F_K_K[0][2]=F_K_K[1][3]=1/exp_theta_y;
    F_K_K[0][3]=F_K_K[2][1]=1/exp_theta_z;
    F_K_K[1][0]=F_K_K[2][3]=exp_theta_x;
    F_K_K[2][0]=F_K_K[3][1]=exp_theta_y;
    F_K_K[3][0]=F_K_K[1][2]=exp_theta_z;                 //计算 F_k_k

    for(i=0;i<7;i++)
    {
        for(j=0;j<7;j++)
        {
            temp_77_1[i][j]=0;
        }
    }                                                   //清空临时变量。

    temp_77_1[0][0]=temp_77_1[1][1]=temp_77_1[2][2]=1;
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            temp_77_1[i+3][j+3]=F_K_K[i][j];
        }
    }                                  
    MatrixMul((float*)temp_77_1,(float*)x_k_k,(float*)x_k1_k,7,7,1);  //对x_k1_k赋值

    for(i=0;i<4;i++)
    {
        q_k1_k[i]=x_k1_k[i];
    }                                    //从x_k1_k中提取q_k1_k

    exp_theta_x=exp(0.5*theta_k[0]);
    exp_theta_y=exp(0.5*theta_k[1]);
    exp_theta_z=exp(0.5*theta_k[2]);
    F_K[0][0]=F_K[1][1]=F_K[2][2]=F_K[3][3]=1;
    F_K[0][1]=F_K[3][2]=1/exp_theta_x;
    F_K[0][2]=F_K[1][3]=1/exp_theta_y;
    F_K[0][3]=F_K[2][1]=1/exp_theta_z;
    F_K[1][0]=F_K[2][3]=exp_theta_x;
    F_K[2][0]=F_K[3][1]=exp_theta_y;
    F_K[3][0]=F_K[1][2]=exp_theta_z;                 //计算 F_k

    X_k_k[0][0]=X_k_k[1][1]=X_k_k[2][2]=q_k_k[0];
    X_k_k[0][1]=X_k_k[3][2]=-1*q_k_k[2];
    X_k_k[0][2]=q_k_k[1];
    X_k_k[1][0]=q_k_k[2];
    X_k_k[1][2]=X_k_k[3][0]=-1*q_k_k[3];
    X_k_k[2][0]=X_k_k[3][1]=-1*q_k_k[1];
    X_k_k[2][1]=q_k_k[3];                            //对X_k_k赋值

    for(i=0;i<7;i++)
    {
        for(j=0;j<7;j++)
        {   
            pusai[i][j]=0;
        }
    }
    pusai[0][0]=pusai[1][1]=pusai[2][2]=1;
    for(i=0;i<4;i++)
    {
        for(j=0;j<3;j++)
        {
            pusai[i+3][j]=-0.5*TWO_TIM*X_k_k[i][j];
        }
    }                    
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            pusai[i+3][j+3]=F_K[i][j];
        }
    }                                        //对pusai赋值

    MatrixMul((float*)q_k_k,(float*)q_k_k,(float*)temp_44_1,4,1,4);
    MatrixAdd((float*)temp_44_1,(float*)p_k_k_q,(float*)M_k_k,4,4);
    tr=M_k_k[0][0]+M_k_k[1][1]+M_k_k[2][2]+M_k_k[3][3];          //计算M_k_k 和  tr 

    for(i=0;i<7;i++)
    {
        for(j=0;j<7;j++)
        {
            p_k_w[i][j]=0;
        }
    }                                              //清空p_k_w
    p_k_w[0][0]=p_k_w[1][1]=p_k_w[2][2]=SIGAMA_3*SIGAMA_3*TWO_TIM;
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            temp_44_1[i][j]=tr*I4[i][j];
        }
    }
    MatrixMinus((float*)temp_44_1,(float*)M_k_k,(float*)temp_44_2,4,4);
    temp=0.25*(SIGAMA_1*SIGAMA_1+SIGAMA_2*SIGAMA_2*TWO_TIM);
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            p_k_w[i+3][j+3]=temp*temp_44_2[i][j];
        }
    }                                               //对p_k_w赋值

    MatrixTrans((float*)pusai,(float*)temp_77_2,7,7);
    MatrixMul((float*)pusai,(float*)p_k_k,(float*)temp_77_1,7,7,7);
    MatrixMul((float*)temp_77_1,(float*)temp_77_2,(float*)temp_77_3,7,7,7);
    MatrixAdd((float*)temp_77_3,(float*)p_k_w,(float*)p_k1_k,7,7); //对p_k1_k赋值

    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            p_k1_k_q[i][j]=p_k1_k[i][j];
        }
    }                                               //从p_k1_k中提取p_k1_k_q


    /*
    r_k1产生随机序列。。。。
    */


    for(i=0;i<3;i++)
    {
        c_k1[i]=0.5*(b_k1[i]+r_k1[i]);
        d_k1[i]=0.5*(b_k1[i]-r_k1[i]);
    }

    H_k1[0][0]=H_k1[1][1]=H_k1[2][2]=H_k1[3][3]=0;
    for(i=0;i<3;i++)
    {
        H_k1[0][i+1]=-1*d_k1[i];
        H_k1[i+1][0]=d_k1[i];
    }
    H_k1[1][2]=   c_k1[2];
    H_k1[1][3]=-1*c_k1[1];
    H_k1[2][1]=-1*c_k1[2];
    H_k1[2][3]=   c_k1[0];
    H_k1[3][1]=   c_k1[1];
    H_k1[3][2]=-1*c_k1[0];         //对 H_k1赋值

    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            H_k1_2[i][j+3]=H_k1[i][j];
        }
    }                              //对H_k1_2赋值

    MatrixMul((float*)q_k1_k,(float*)q_k1_k,(float*)temp_44_1,4,1,4);
    MatrixAdd((float*)temp_44_1,(float*)p_k1_k_q,(float*)M_k1_k,4,4);  //计算M_k1_k
    tr=M_k1_k[0][0]+M_k1_k[1][1]+M_k1_k[2][2]+M_k1_k[3][3];   //计算 tr

    B_k1[0][0]=B_k1[1][1]=B_k1[2][2]=B_k1[3][3]=0;
    B_k1[0][1]=B_k1[3][2]=-1*b_k1[0];
    B_k1[0][2]=B_k1[1][3]=-1*b_k1[1];
    B_k1[0][3]=B_k1[2][1]=-1*b_k1[2];
    B_k1[1][0]=B_k1[2][3]=b_k1[0];
    B_k1[2][0]=B_k1[3][1]=b_k1[1];
    B_k1[3][0]=B_k1[1][2]=b_k1[2]; //对 B_k1 赋值 

    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            temp_44_1[i][j]=0;
            temp_44_2[i][j]=0;
            temp_44_3[i][j]=0;
        }
    }
    temp_44_1[0][0]=temp_44_1[1][1]=temp_44_1[2][2]=temp_44_1[3][3]=tr;
    MatrixMinus((float*)temp_44_1,(float*)M_k1_k,(float*)temp_44_2,4,4);
    MatrixTrans((float*)B_k1,(float*)temp_44_1,4,4);
    MatrixMul((float*)B_k1,(float*)M_k1_k,(float*)temp_44_3,4,4,4);
    MatrixMul((float*)temp_44_3,(float*)temp_44_1,(float*)temp_44_4,4,4,4);
    MatrixMinus((float*)temp_44_2,(float*)temp_44_4,(float*)temp_44_1,4,4);
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            p_k1_k_v[i][j]=0.25*ROU_K*temp_44_1[i][j];
        }
    }                                                                 //计算p_k1_k_v

    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            temp_44_1[i][j]=0;
            temp_44_2[i][j]=0;
            temp_44_3[i][j]=0;
        }
    }
    MatrixTrans((float*)H_k1,(float*)temp_44_4,4,4);
    MatrixMul((float*)H_k1,(float*)p_k1_k_q,(float*)temp_44_1,4,4,4);
    MatrixMul((float*)temp_44_1,(float*)temp_44_4,(float*)temp_44_2,4,4,4);
    MatrixAdd((float*)temp_44_2,(float*)p_k1_k_v,(float*)S_k1_k,4,4);   //计算S_k1_k

    MatrixTrans((float*)H_k1_2,(float*)temp_74,4,7);
    MatrixMul((float*)p_k1_k,(float*)temp_74,(float*)temp_74_2,7,7,4);
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            temp_44_1[i][j]=S_k1_k[i][j];
        }
    }
    Gauss_Jordan((float*)temp_44_1,4);
    MatrixMul((float*)temp_74_2,(float*)temp_44_1,(float*)K_k1,7,4,4);      //计算K_k1

    MatrixMul((float*)K_k1,(float*)H_k1_2,(float*)temp_77_3,7,4,7);
    MatrixMinus((float*)I7,(float*)temp_77_3,(float*)temp_77_1,7,7);
    MatrixTrans((float*)temp_77_1,(float*)temp_77_2,7,7);
    MatrixMul((float*)temp_77_1,(float*)p_k1_k,(float*)temp_77_3,7,7,7);
    MatrixMul((float*)temp_77_3,(float*)temp_77_2,(float*)temp_77_4,7,7,7); 
    MatrixMul((float*)K_k1,(float*)p_k1_k_v,(float*)temp_74,7,4,4);     
    MatrixTrans((float*)K_k1,(float*)temp_47_1,7,4);
    MatrixMul((float*)temp_74,(float*)temp_47_1,(float*)temp_77_3,7,4,7);
    MatrixAdd((float*)temp_77_4,(float*)temp_77_3,(float*)p_k1_k1,7,7);   //计算p_k1_k1

    for(i=0;i<7;i++)
    {
        for(j=0;j<7;j++)
        {
            p_k_k[i][j]=p_k1_k1[i][j];
        }
    }                                             //p_k1_k1保存为下一次的p_k_k
    for(i=0;i<4;i++)
    {
        for(j=0;j<4;j++)
        {
            p_k_k_q[i][j]=p_k1_k1[i][j];
        }
    }                                            //从p_k1_k1中提取p_k_k_q

    MatrixMul((float*)temp_77_1,(float*)x_k1_k,(float*)x_k1_k1,7,7,1);   //计算x_k1_k1
    for(i=0;i<3;i++)
    {
        u_k_k[i]=x_k1_k1[i+4];
    }                                                                    //从x_k1_k1中提取u_k_k

    for(i=0;i<4;i++)
    {
        q_k1_k1[i]=x_k1_k1[i];
    }
    temp=pow(q_k1_k1[0]*q_k1_k1[0]+q_k1_k1[1]*q_k1_k1[1]+q_k1_k1[2]*q_k1_k1[2]+q_k1_k1[3]*q_k1_k1[3],0.5);
    for(i=0;i<4;i++)
    {
        q_k_k[i]=q_k1_k1[i]/temp;
    }                                                                    //从x_k1_k1中提取四元数归一化并保存为下一次的四元数
}

参考文献

[1]关治，陆金甫.数值分析基础[M].同济大学出版社.1998：75-80.

[2]关治，陆金甫.数值分析基础[M].同济大学出版社.1998：38-41.

[3]关治，陆金甫.数值分析基础[M].同济大学出版社.1998：59-60.

[4]杨丕文.四元数分析与偏微分方程[M].科学出版社.2009.

[5]秦永元，汪叔华.卡尔曼滤波与组合导航原理（第二版）[M].西北工业大学出版社.2012.

[6]邓正隆.惯性技术[M].西北工业大学出版社.2006.

[7]张天光，王秀萍.捷联惯导技术[M].国防工业出版社.2007.