用C写出卡尔曼滤波

以下是用C语言实现的基本卡尔曼滤波算法：

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

#define N 2     // 状态变量个数
#define M 1     // 观测变量个数

void kalman_filter(double *x_hat, double *P, double *z, double u, double *A, double *B, double *H, double *Q, double *R);

int main(void)
{
    // 初始状态向量
    double x_hat[N] = {0.0, 0.0};
    // 初始协方差矩阵
    double P[N][N] = {{1.0, 0.0}, {0.0, 1.0}};
    // 测量向量
    double z[M] = {0.0};
    // 输入向量
    double u = 0.0;

    // 系统动态模型矩阵
    double A[N][N] = {{1.0, 1.0}, {0.0, 1.0}};
    // 控制输入矩阵
    double B[N] = {0.0, 0.0};
    // 观测模型矩阵
    double H[M][N] = {{1.0, 0.0}};
    // 状态噪声协方差矩阵
    double Q[N][N] = {{0.01, 0.0}, {0.0, 0.01}};
    // 测量噪声协方差矩阵
    double R[M][M] = {{0.1}};

    // 读取数据并应用卡尔曼滤波
    for (int i = 0; i < 100; i++) {
        // 读取测量数据z和控制输入u
        scanf("%lf %lf", &z[0], &u);
        // 对状态向量和协方差矩阵进行卡尔曼滤波
        kalman_filter(x_hat, (double*)P, z, u, (double*)A, B, (double*)H, (double*)Q, (double*)R);
        // 输出卡尔曼滤波结果
        printf("%lf %lf\n", x_hat[0], x_hat[1]);
    }

    return 0;
}

/**
 * Kalman Filter
 * x_hat: 状态向量估计值
 * P: 状态协方差矩阵
 * z: 测量向量
 * u: 输入量
 * A: 系统动态模型矩阵
 * B: 控制输入矩阵
 * H: 观测模型矩阵
 * Q: 状态噪声协方差矩阵
 * R: 测量噪声协方差矩阵
 */
void kalman_filter(double *x_hat, double *P, double *z, double u, double *A, double *B, double *H, double *Q, double *R)
{
    // 预测阶段
    double x_hat_priori[N] = {0.0};
    for (int i = 0; i < N; i++) {
        x_hat_priori[i] = 0.0;
        for (int j = 0; j < N; j++) {
            x_hat_priori[i] += A[i*N + j] * x_hat[j];
        }
        x_hat_priori[i] += B[i] * u;
    }

    double P_priori[N][N] = {{0.0}};
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < N; j++) {
            P_priori[i][j] = 0.0;
            for (int k = 0; k < N; k++) {
                P_priori[i][j] += A[i*N + k] * P[k*N + j] * A[j*N + k];
            }
            P_priori[i][j] += Q[i*N + j];
        }
    }

    // 更新阶段
    double K[N][M] = {{0.0}};
    double S[M][M] = {{0.0}};
    double y[M] = {0.0};
    for (int i = 0; i < M; i++) {
        y[i] = z[i];
        for (int j = 0; j < N; j++) {
            y[i] -= H[i*N + j] * x_hat_priori[j];
        }
    }

    for (int i = 0; i < M; i++) {
        for (int j = 0; j < M; j++) {
            S[i][j] = 0.0;
            for (int k = 0; k < N; k++) {
                S[i][j] += H[i*N + k] * P_priori[k*N + j] * H[j*N + k];
            }
            S[i][j] += R[i*M + j];
        }
    }

    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            K[i][j] = 0.0;
            for (int k = 0; k < N; k++) {
                K[i][j] += P_priori[i*N + k] * H[j*N + k];
            }
            K[i][j] /= S[j][j];
        }
    }

    for (int i = 0; i < N; i++) {
        x_hat[i] = x_hat_priori[i];
        for (int j = 0; j < M; j++) {
            x_hat[i] += K[i][j] * y[j];
        }
    }

    for (int i = 0; i < N; i++) {
        for (int j = 0; j < N; j++) {
            P[i*N + j] = P_priori[i][j];
            for (int k = 0; k < M; k++) {
                P[i*N + j] -= K[i][k] * H[k*N + j] * P_priori[i*N + j];
            }
        }
    }
}

代码中实现了一个简单的卡尔曼滤波算法，用于处理带有噪声的一维位置测量数据。在每次循环中，程序读取测量值和控制输入，并将其输入到卡尔曼滤波器中进行处理，最后输出卡尔曼滤波器的估计结果。

该实现中使用了一个二维状态向量和一个一维测量向量，状态向量的第一个分量表示位置，第二个分量表示速度。协方差矩阵用一个二维数组表示，其中P[i][j]表示状态向量第i个分量和第j个分量之间的协方差。系统动态模型矩阵A、控制输入矩阵B、观测模型矩阵H、状态噪声协方差矩阵Q和测量噪声协方差矩阵R也都用二维数组表示。