二维卡尔曼滤波c语言实现

二维卡尔曼滤波是一种用于估计未知状态的滤波算法，同时考虑系统模型的预测值和测量值的协方差。下面是一个简单的二维卡尔曼滤波的C语言实现示例。

#include <stdio.h>

// 状态矩阵
double X[2] = {0, 0}; 

// 状态协方差矩阵
double P[4] = {1, 0, 0, 1};

// 系统模型
double A[4] = {1, 1, 0, 1};

// 测量矩阵
double C[2] = {1, 0};

// 测量噪声
double R[1] = {0.001};

// 系统噪声
double Q[4] = {0.01, 0.001, 0.001, 0.01};

// 卡尔曼增益
double K[2] = {0, 0};

void kalman_filter(double z) {
    // 预测
    double X_pre[2] = {A[0] * X[0] + A[1] * X[1], A[2] * X[0] + A[3] * X[1]};
    double P_pre[4] = {A[0] * P[0] + A[1] * P[2], A[0] * P[1] + A[1] * P[3], A[2] * P[0] + A[3] * P[2], A[2] * P[1] + A[3] * P[3]};

    // 计算卡尔曼增益
    double S = C[0] * P_pre[0] + C[1] * P_pre[2] + R[0];
    K[0] = P_pre[0] * C[0] / S;
    K[1] = P_pre[2] * C[1] / S;

    // 更新状态估计值和协方差
    X[0] = X_pre[0] + K[0] * (z - C[0] * X_pre[0]);
    X[1] = X_pre[1] + K[1] * (z - C[0] * X_pre[0]);
    P[0] = P_pre[0] - K[0] * C[0] * P_pre[0];
    P[1] = P_pre[1] - K[0] * C[0] * P_pre[1];
    P[2] = P_pre[2] - K[1] * C[1] * P_pre[0];
    P[3] = P_pre[3] - K[1] * C[1] * P_pre[1];
}

int main() {
    // 测量值
    double z = 1.5;

    // 进行滤波
    kalman_filter(z);
    
    // 输出估计值
    printf("Estimated state: X1 = %lf, X2 = %lf\n", X[0], X[1]);

    return 0;
}

以上代码实现了一个简单的二维卡尔曼滤波的C语言实现。首先定义了状态矩阵X，状态协方差矩阵P，系统模型A，测量矩阵C，测量噪声R，系统噪声Q和卡尔曼增益K。然后实现了一个`kalman_filter`函数，用于进行滤波。在`main`函数中，定义了一个测量值z，然后调用`kalman_filter`函数得到估计值X，并输出到控制台。