卡尔曼滤波的代码实现

卡尔曼滤波是一种利用状态预测值和测量值进行最优估计的算法，可以用于估计系统的状态。以下是一个C语言实现的卡尔曼滤波的代码示例[^1][^2]：

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

#define Q 0.0001 // 过程噪声协方差
#define R 0.1 // 测量噪声协方差

// 卡尔曼滤波器的状态结构体
typedef struct {
    double x_hat; // 状态估计值
    double p; // 状态估计协方差
} kalman_state;

// 初始化滤波器
void kalman_init(kalman_state *state, double x_init, double p_init) {
    state->x_hat = x_init;
    state->p = p_init;
}

// 卡尔曼滤波器的状态更新
void kalman_update(kalman_state *state, double z, double u) {
    // 状态预测
    double x_hat_minus = state->x_hat + u;
    double p_minus = state->p + Q;
    
    // 计算卡尔曼增益
    double k = p_minus / (p_minus + R);
    
    // 状态更新
    state->x_hat = x_hat_minus + k * (z - x_hat_minus);
    state->p = (1 - k) * p_minus;
}

int main() {
    double z = 0.5; // 测量值
    double u = 0; // 外部输入
    double x_init = 0; // 初始状态估计值
    double p_init = 1; // 初始状态估计协方差
    
    // 初始化滤波器状态
    kalman_state state;
    kalman_init(&state, x_init, p_init);
    
    // 循环更新状态
    for (int i = 0; i < 10; i++) {
        kalman_update(&state, z, u);
        printf("The %d-th estimated state is: %lf\n", i, state.x_hat);
    }
    return 0;
}

该代码实现了一个简单的一维卡尔曼滤波器，包括状态预测、卡尔曼增益计算和状态更新等步骤。在实际应用中，还需要根据具体的系统需求进行修改和优化。