用C语言写一个卡尔曼滤波器扩展算法程序

以下是一个简单的卡尔曼滤波器扩展算法程序的示例代码：

#include <stdio.h>

#define N_STATE 2   // 状态数量
#define N_OBSRV 1   // 观测值数量

void kalman_filter(double x[N_STATE], double P[N_STATE][N_STATE],
                   double z[N_OBSRV], double R[N_OBSRV][N_OBSRV],
                   double Q[N_STATE][N_STATE], double F[N_STATE][N_STATE],
                   double H[N_OBSRV][N_STATE], double y[N_OBSRV],
                   double S[N_OBSRV][N_OBSRV], double K[N_STATE][N_OBSRV])
{
    // 预测步骤
    double x_pred[N_STATE];
    double P_pred[N_STATE][N_STATE];
    for (int i = 0; i < N_STATE; i++) {
        x_pred[i] = 0.0;
        for (int j = 0; j < N_STATE; j++) {
            x_pred[i] += F[i][j] * x[j];
        }
    }

    for (int i = 0; i < N_STATE; i++) {
        for (int j = 0; j < N_STATE; j++) {
            P_pred[i][j] = 0.0;
            for (int k = 0; k < N_STATE; k++) {
                P_pred[i][j] += F[i][k] * P[k][j] * F[j][k];
            }
            P_pred[i][j] += Q[i][j];
        }
    }

    // 更新步骤
    double y_pred[N_OBSRV];
    for (int i = 0; i < N_OBSRV; i++) {
        y_pred[i] = 0.0;
        for (int j = 0; j < N_STATE; j++) {
            y_pred[i] += H[i][j] * x_pred[j];
        }
    }

    for (int i = 0; i < N_OBSRV; i++) {
        for (int j = 0; j < N_OBSRV; j++) {
            S[i][j] = 0.0;
            for (int k = 0; k < N_STATE; k++) {
                S[i][j] += H[i][k] * P_pred[k][j] * H[j][k];
            }
            S[i][j] += R[i][j];
        }
    }

    for (int i = 0; i < N_STATE; i++) {
        for (int j = 0; j < N_OBSRV; j++) {
            K[i][j] = 0.0;
            for (int k = 0; k < N_OBSRV; k++) {
                K[i][j] += P_pred[i][k] * H[k][j] / S[k][j];
            }
        }
    }

    for (int i = 0; i < N_STATE; i++) {
        x[i] = x_pred[i] + K[i][0] * (z[0] - y_pred[0]);
        for (int j = 0; j < N_STATE; j++) {
            P[i][j] = P_pred[i][j] - K[i][0] * S[0][j] * K[j][0];
        }
    }
}

int main()
{
    // 初始状态
    double x[N_STATE] = {0.0, 0.0};
    double P[N_STATE][N_STATE] = {{1.0, 0.0}, {0.0, 1.0}};

    // 观测值
    double z[N_OBSRV] = {1.0};
    double R[N_OBSRV][N_OBSRV] = {{1.0}};

    // 系统和观测误差协方差矩阵
    double Q[N_STATE][N_STATE] = {{0.1, 0.0}, {0.0, 0.1}};
    double F[N_STATE][N_STATE] = {{1.0, 1.0}, {0.0, 1.0}};
    double H[N_OBSRV][N_STATE] = {{1.0, 0.0}};

    // 进行卡尔曼滤波
    double y[N_OBSRV];
    double S[N_OBSRV][N_OBSRV];
    double K[N_STATE][N_OBSRV];

    for (int i = 0; i < 10; i++) {
        kalman_filter(x, P, z, R, Q, F, H, y, S, K);
        printf("x = %f, P = %f\n", x[0], P[0][0]);
    }

    return 0;
}

此程序包含了一个简单的二维状态估计问题，并使用卡尔曼滤波器扩展算法进行估计。在实践中，您需要根据您的具体问题定义状态变量、观测变量、系统和观测误差的协方差矩阵等参数。