卡尔曼滤波C语言代码v2.0

卡尔曼滤波C语言代码v2.0

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

// 定义常量
#define dt 1.0  // 时间步长
#define Q 0.01  // 过程噪声协方差
#define R 0.1   // 测量噪声协方差

// 状态向量 [位置，速度]
typedef struct {
    double position;
    double velocity;
} State;

// 协方差矩阵
typedef struct {
    double P00, P01;
    double P10, P11;
} Covariance;

// 初始化卡尔曼滤波器
void Kalman_Init(State *state, Covariance *P) {
    state->position = 0.0;  // 初始位置
    state->velocity = 0.0;  // 初始速度
    P->P00 = 1.0;           // 初始位置协方差
    P->P01 = 0.0;
    P->P10 = 0.0;
    P->P11 = 1.0;           // 初始速度协方差
}

// 预测步骤
void Kalman_Predict(State *state, Covariance *P) {
    // 状态预测
    double temp_position = state->position;
    double temp_velocity = state->velocity;
    state->position = temp_position + dt * temp_velocity;  // 位置 = 位置 + 速度 * 时间步长
    state->velocity = temp_velocity;                       // 速度保持不变

    // 协方差预测
    double temp_P00 = P->P00;
    double temp_P01 = P->P01;
    double temp_P10 = P->P10;
    double temp_P11 = P->P11;

    P->P00 = temp_P00 + dt * temp_P01 + dt * temp_P10 + dt * dt * temp_P11 + Q;
    P->P01 = temp_P01 + dt * temp_P11;
    P->P10 = temp_P10;
    P->P11 = temp_P11 + Q;
}

// 更新步骤
void Kalman_Update(State *state, Covariance *P, double measurement) {
    // 测量矩阵 H = [1, 0]，只测量位置
    double H00 = 1.0;
    double H01 = 0.0;

    // 计算卡尔曼增益
    double S = H00 * P->P00 + H01 * P->P10;  // 测量残差协方差
    double K0 = (H00 * P->P00 + H01 * P->P10) / S;
    double K1 = (H00 * P->P01 + H01 * P->P11) / S;

    // 测量残差
    static double y = 0.0;
    //printf("00  measurement:%f\n", measurement);
    if(measurement < 1.0)
    {
        //y = y;//state->position;
        //printf("\n11 y:%f positionlast:%f position:%f\n",y, stateLast->position, state->position);
    }else{
        y = measurement - state->position;
        //printf("22 y:%f measurement:%f position:%f\n",y, measurement, state->position);
    }

    // 状态更新
    state->position += K0 * y;
    state->velocity += K1 * y;

    // 协方差更新
    P->P00 -= K0 * H00 * P->P00 + K0 * H01 * P->P10;
    P->P01 -= K0 * H00 * P->P01 + K0 * H01 * P->P11;
    P->P10 -= K1 * H00 * P->P00 + K1 * H01 * P->P10;
    P->P11 -= K1 * H00 * P->P01 + K1 * H01 * P->P11;
}

int main() {
    // 初始化状态和协方差
    State state;
    Covariance P;
    Kalman_Init(&state, &P);
    // 模拟测量数据（带噪声）
    double measurements[] = {1.2, 2.1, 3.0, 4.1, 5.0, 3.1, 7.0, 8.1, 9.0, 10.1, 0,0,0,0,0};
    int num_measurements = sizeof(measurements) / sizeof(measurements[0]);

    // 卡尔曼滤波循环
    for (int k = 0; k < num_measurements; k++) {
        Kalman_Predict(&state, &P);  // 预测
        Kalman_Update(&state, &P, measurements[k]);  // 更新
        printf("Time: %d, Position: %.2f, Velocity: %.2f\n", k, state.position, state.velocity);
    }

    return 0;
}