EKF c语言 姿态融合算法

以下是一个使用EKF算法实现姿态融合的C语言示例代码：

#include <stdio.h>
#include <math.h>

#define PI 3.14159265358979323846

// 定义状态向量
typedef struct {
    double q0, q1, q2, q3; // 四元数
    double wx, wy, wz; // 陀螺仪测量值
} StateVector;

// 定义观测向量
typedef struct {
    double mx, my, mz; // 磁力计测量值
    double ax, ay, az; // 加速度计测量值
} ObservationVector;

// 定义协方差矩阵
typedef struct {
    double p[7][7];
} CovarianceMatrix;

// 定义常量
const double dt = 0.01; // 采样周期
const double Q = 0.1; // 系统噪声协方差
const double R = 0.5; // 观测噪声协方差

// 定义函数
void EKF(StateVector *x, CovarianceMatrix *P, ObservationVector z, double dt);
void quaternion_normalize(StateVector *x);
void quaternion_multiply(StateVector *x, StateVector q);
void quaternion_update(StateVector *x, ObservationVector z);

int main() {
    // 初始化状态向量
    StateVector x = {1, 0, 0, 0, 0, 0, 0};
    // 初始化协方差矩阵
    CovarianceMatrix P = {
        .p = {
            {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}
        }
    };
    // 初始化观测向量
    ObservationVector z = {0.1, 0.2, 0.3, 0, 0, 9.8};
    // 进行EKF算法
    EKF(&x, &P, z, dt);
    // 输出结果
    printf("q0 = %f, q1 = %f, q2 = %f, q3 = %f\n", x.q0, x.q1, x.q2, x.q3);
    printf("wx = %f, wy = %f, wz = %f\n", x.wx, x.wy, x.wz);
    // 返回
    return 0;
}

// EKF算法
void EKF(StateVector *x, CovarianceMatrix *P, ObservationVector z, double dt) {
    // 定义中间变量
    StateVector x_pred;
    CovarianceMatrix P_pred;
    CovarianceMatrix F;
    CovarianceMatrix Q;
    CovarianceMatrix H;
    CovarianceMatrix R;
    CovarianceMatrix K;
    ObservationVector z_pred;
    double y[6];
    double S[6][6];
    double det_S;
    double inv_S[6][6];
    double temp[7][6];
    // 预测状态
    x_pred.q0 = x->q0 + 0.5 * (-x->q1 * x->wx - x->q2 * x->wy - x->q3 * x->wz) * dt;
    x_pred.q1 = x->q1 + 0.5 * (x->q0 * x->wx + x->q2 * x->wz - x->q3 * x->wy) * dt;
    x_pred.q2 = x->q2 + 0.5 * (x->q0 * x->wy - x->q1 * x->wz + x->q3 * x->wx) * dt;
    x_pred.q3 = x->q3 + 0.5 * (x->q0 * x->wz + x->q1 * x->wy - x->q2 * x->wx) * dt;
    x_pred.wx = x->wx;
    x_pred.wy = x->wy;
    x_pred.wz = x->wz;
    // 预测协方差
    F.p[0][0] = 1;
    F.p[0][1] = -0.5 * x->wx * dt;
    F.p[0][2] = -0.5 * x->wy * dt;
    F.p[0][3] = -0.5 * x->wz * dt;
    F.p[0][4] = 0;
    F.p[0][5] = 0;
    F.p[0][6] = 0;
    F.p[1][0] = 0.5 * x->wx * dt;
    F.p[1][1] = 1;
    F.p[1][2] = 0.5 * x->wz * dt;
    F.p[1][3] = -0.5 * x->wy * dt;
    F.p[1][4] = 0;
    F.p[1][5] = 0;
    F.p[1][6] = 0;
    F.p[2][0] = 0.5 * x->wy * dt;
    F.p[2][1] = -0.5 * x->wz * dt;
    F.p[2][2] = 1;
    F.p[2][3] = 0.5 * x->wx * dt;
    F.p[2][4] = 0;
    F.p[2][5] = 0;
    F.p[2][6] = 0;
    F.p[3][0] = 0.5 * x->wz * dt;
    F.p[3][1] = 0.5 * x->wy * dt;
    F.p[3][2] = -0.5 * x->wx * dt;
    F.p[3][3] = 1;
    F.p[3][4] = 0;
    F.p[3][5] = 0;
    F.p[3][6] = 0;
    F.p[4][0] = 0;
    F.p[4][1] = 0;
    F.p[4][2] = 0;
    F.p[4][3] = 0;
    F.p[4][4] = 1;
    F.p[4][5] = 0;
    F.p[4][6] = 0;
    F.p[5][0] = 0;
    F.p[5][1] = 0;
    F.p[5][2] = 0;
    F.p[5][3] = 0;
    F.p[5][4] = 0;
    F.p[5][5] = 1;
    F.p[5][6] = 0;
    F.p[6][0] = 0;
    F.p[6][1] = 0;
    F.p[6][2] = 0;
    F.p[6][3] = 0;
    F.p[6][4] = 0;
    F.p[6][5] = 0;
    F.p[6][6] = 1;
    Q.p[0][0] = Q;
    Q.p[1][1] = Q;
    Q.p[2][2] = Q;
    Q.p[3][3] = Q;
    Q.p[4][4] = Q;
    Q.p[5][5] = Q;
    Q.p[6][6] = Q;
    P_pred = *P;
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            for (int k = 0; k < 7; k++) {
                P_pred.p[i][j] += F.p[i][k] * P->p[k][j];
            }
        }
    }
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            for (int k = 0; k < 7; k++) {
                P_pred.p[i][j] += P->p[i][k] * F.p[j][k];
            }
        }
    }
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            for (int k = 0; k < 7; k++) {
                Q.p[i][j] += F.p[i][k] * Q.p[k][j];
            }
        }
    }
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            for (int k = 0; k < 7; k++) {
                Q.p[i][j] += Q.p[i][k] * F.p[j][k];
            }
        }
    }
    // 更新状态
    quaternion_multiply(&x_pred, *x);
    quaternion_normalize(&x_pred);
    // 更新协方差
    H.p[0][0] = 2 * x->q0;
    H.p[0][1] = -2 * x->q3;
    H.p[0][2] = 2 * x->q2;
    H.p[0][3] = -2 * x->q1;
    H.p[0][4] = 0;
    H.p[0][5] = 0;
    H.p[0][6] = 0;
    H.p[1][0] = 2 * x->q1;
    H.p[1][1] = 2 * x->q2;
    H.p[1][2] = 2 * x->q3;
    H.p[1][3] = 2 * x->q0;
    H.p[1][4] = 0;
    H.p[1][5] = 0;
    H.p[1][6] = 0;
    H.p[2][0] = -2 * x->q2;
    H.p[2][1] = 2 * x->q1;
    H.p[2][2] = 2 * x->q0;
    H.p[2][3] = 2 * x->q3;
    H.p[2][4] = 0;
    H.p[2][5] = 0;
    H.p[2][6] = 0;
    H.p[3][0] = -2 * x->q3;
    H.p[3][1] = -2 * x->q0;
    H.p[3][2] = 2 * x->q1;
    H.p[3][3] = 2 * x->q2;
    H.p[3][4] = 0;
    H.p[3][5] = 0;
    H.p[3][6] = 0;
    H.p[4][0] = 0;
    H.p[4][1] = 0;
    H.p[4][2] = 0;
    H.p[4][3] = 0;
    H.p[4][4] = 1;
    H.p[4][5] = 0;
    H.p[4][6] = 0;
    H.p[5][0] = 0;
    H.p[5][1] = 0;
    H.p[5][2] = 0;
    H.p[5][3] = 0;
    H.p[5][4] = 0;
    H.p[5][5] = 1;
    H.p[5][6] = 0;
    R.p[0][0] = R;
    R.p[1][1] = R;
    R.p[2][2] = R;
    R.p[3][3] = R;
    R.p[4][4] = R;
    R.p[5][5] = R;
    for (int i = 0; i < 6; i++) {
        z_pred.mx = 2 * (x_pred.q1 * x_pred.q3 - x_pred.q0 * x_pred.q2);
        z_pred.my = 2 * (x_pred.q0 * x_pred.q1 + x_pred.q2 * x_pred.q3);
        z_pred.mz = x_pred.q0 * x_pred.q0 - x_pred.q1 * x_pred.q1 - x_pred.q2 * x_pred.q2 + x_pred.q3 * x_pred.q3;
        z_pred.ax = 2 * (x_pred.q1 * x_pred.q3 + x_pred.q0 * x_pred.q2);
        z_pred.ay = 2 * (x_pred.q0 * x_pred.q1 - x_pred.q2 * x_pred.q3);
        z_pred.az = x_pred.q0 * x_pred.q0 + x_pred.q1 * x_pred.q1 - x_pred.q2 * x_pred.q2 - x_pred.q3 * x_pred.q3;
        y[i] = z_pred.mx - z.mx;
        y[i + 3] = z_pred.my - z.my;
    }
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 7; j++) {
            temp[j][i