代码示例：
kalman.c
- #include <stdio.h>
- #include <math.h>
- typedef struct {
- float x; // 状态估计值
- float P; // 估计误差协方差
- float Q; // 过程噪声协方差
- float R; // 测量噪声协方差
- float K; // 卡尔曼增益
- } KalmanFilter;
- // 初始化滤波器
- KalmanFilter kalman_init(float x_init, float P_init, float Q, float R) {
- KalmanFilter kf;
- kf.x = x_init;
- kf.P = P_init;
- kf.Q = Q;
- kf.R = R;
- return kf;
- }
- // 预测步骤
- void kalman_predict(KalmanFilter* kf) {
- kf->x = kf->x; // 状态预测（假设系统为恒定模型）
- kf->P += kf->Q; // 协方差预测
- }
- // 更新步骤
- void kalman_update(KalmanFilter* kf, float z) {
- kf->K = kf->P / (kf->P + kf->R); // 计算卡尔曼增益
- kf->x += kf->K * (z - kf->x); // 状态更新
- kf->P *= (1 - kf->K); // 协方差更新
- }
- /* 使用示例：
- KalmanFilter kf = kalman_init(0, 1, 0.1, 0.1);
- while(1) {
- kalman_predict(&kf);
- float measurement = get_sensor_data();
- kalman_update(&kf, measurement);
- }
- */
| 
楼主
