用c++写一篇卡尔曼滤波

好的，以下是一个简单的卡尔曼滤波器的C代码实现：

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

#define STATE_DIM 2  // 状态向量维度
#define MEAS_DIM 1   // 测量向量维度

typedef struct {
    double x[STATE_DIM];   // 状态向量
    double P[STATE_DIM][STATE_DIM];   // 状态协方差矩阵
} state_t;

typedef struct {
    double z[MEAS_DIM];    // 测量向量
    double R[MEAS_DIM][MEAS_DIM];    // 测量噪声协方差矩阵
} measurement_t;

typedef struct {
    double F[STATE_DIM][STATE_DIM];  // 状态转移矩阵
    double B[STATE_DIM];    // 控制变量矩阵
    double Q[STATE_DIM][STATE_DIM];  // 状态噪声协方差矩阵
} model_t;

// 卡尔曼滤波器
void kalman_filter(state_t* x, measurement_t* z, model_t* model) {
    double K[STATE_DIM][MEAS_DIM];  // 卡尔曼增益矩阵
    double S[MEAS_DIM][MEAS_DIM];   // 测量噪声协方差矩阵
    double y[MEAS_DIM]; // 测量残差

    // 预测状态
    for (int i = 0; i < STATE_DIM; i++) {
        x->x[i] = model->F[i][0] * x->x[0] + model->F[i][1] * x->x[1] + model->B[i];
        for (int j = 0; j < STATE_DIM; j++) {
            x->P[i][j] = model->F[i][0] * x->P[0][j] + model->F[i][1] * x->P[1][j] + model->Q[i][j];
        }
    }

    // 计算卡尔曼增益
    for (int i = 0; i < MEAS_DIM; i++) {
        for (int j = 0; j < STATE_DIM; j++) {
            S[i][j] = model->R[i][0] * x->P[0][j] + model->R[i][1] * x->P[1][j];
        }
    }
    double inv_S = 1.0 / (S[0][0] + model->R[0][0]);
    K[0][0] = x->P[0][0] * inv_S;
    K[1][0] = x->P[1][0] * inv_S;

    // 更新状态
    y[0] = z->z[0] - (x->x[0] * model->F[0][0] + x->x[1] * model->F[0][1] + model->B[0]);
    x->x[0] += K[0][0] * y[0];
    x->x[1] += K[1][0] * y[0];
    x->P[0][0] -= K[0][0] * S[0][0] * K[0][0];
    x->P[0][1] -= K[0][0] * S[0][0] * K[1][0];
    x->P[1][0] -= K[1][0] * S[0][0] * K[0][0];
    x->P[1][1] -= K[1][0] * S[0][0] * K[1][0];
}

int main() {
    state_t x = {
        .x = {0.0, 0.0},
        .P = {{1.0, 0.0}, {0.0, 1.0}}
    };
    measurement_t z = {
        .z = {1.0},
        .R = {{0.1}}
    };
    model_t model = {
        .F = {{1.0, 1.0}, {0.0, 1.0}},
        .B = {0.0, 0.0},
        .Q = {{0.01, 0.0}, {0.0, 0.01}},
        .R = {{0.1}}
    };
    for (int i = 0; i < 10; i++) {
        kalman_filter(&x, &z, &model);
        printf("%f, %f\n", x.x[0], x.x[1]);
    }
    return 0;
}

这个代码实现了一个简单的一维卡尔曼滤波器，具有状态向量维度为2，测量向量维度为1，状态转移矩阵为[[1,1],[0,1]]，控制变量矩阵为[0,0]，初始状态向量为[0,0]，初始状态协方差矩阵为[[1,0],[0,1]]，测量噪声协方差矩阵为[[0.1]]，状态噪声协方差矩阵为[[0.01,0],[0,0.01]]。在main函数中，我们对这个滤波器进行了10次迭代，并输出了每次迭代后的状态向量。