C语言实现卡尔曼滤波（附带源码）

最新推荐文章于 2025-03-30 11:58:38 发布

Katie。

最新推荐文章于 2025-03-30 11:58:38 发布

阅读量1.9k

点赞数 29

分类专栏： c、c++实战项目文章标签：机器学习人工智能

本文链接：https://blog.csdn.net/m0_61840987/article/details/145430111

版权

c、c++实战项目专栏收录该内容

510 篇文章

订阅专栏

C 语言实现卡尔曼滤波项目详解

一、项目背景与概述

卡尔曼滤波器是一种递归算法，用于在有噪声的测量数据中估计系统状态。它在控制系统、信号处理、导航与定位等领域有着广泛的应用。卡尔曼滤波器利用预测与更新两大步骤，将测量值与系统模型相结合，从而在噪声背景下获得更精确的状态估计。

本项目采用 C 语言实现一个简单的一维卡尔曼滤波器，适用于对一个随时间变化的标量状态进行估计。示例中假设系统状态为一个常数（例如某个物理量的稳定值），并引入过程噪声和测量噪声。程序将通过一系列带噪声的测量值，通过卡尔曼滤波获得状态的最佳估计。

项目主要内容包括：

数据结构设计：定义卡尔曼滤波器数据结构，保存状态估计、估计误差协方差以及噪声协方差等参数。
滤波器初始化：设置初始状态、误差协方差、过程噪声和测量噪声等参数。
预测与更新算法实现：分别实现卡尔曼滤波器的预测（Prediction）和更新（Update）步骤。
示例测试：在主函数中模拟一系列带噪声的测量值，通过卡尔曼滤波获得状态估计，并输出对比原始测量值与滤波结果。

该项目适合作为 C 语言初学者了解状态估计和滤波算法的入门案例，同时也为更复杂的多维卡尔曼滤波提供基础。

二、项目实现思路

1. 卡尔曼滤波器数学原理（以一维为例）

对于一维系统，设系统状态为 x ，系统模型简单设为：

对于本项目，假设系统状态为常数（状态转移系数 A=1），没有控制输入（B u_k = 0），测量直接观测状态（H=1）。

2. 算法实现流程

结果输出：
- 输出每个时刻的滤波器状态估计以及卡尔曼增益 K_k 等信息，与原始测量值对比。

3. 数据结构设计

定义一个结构体 KalmanFilter 来保存：

当前状态估计 x
当前估计误差协方差 P
过程噪声协方差 Q
测量噪声协方差 R
卡尔曼增益 K

三、完整代码（附详细注释）

下面给出完整的 C 语言代码示例，实现了简单的一维卡尔曼滤波器。代码中包含详细注释，解释每个步骤的实现原理。

/********************************************************************
 * C语言实现一维卡尔曼滤波器
 *
 * 本程序实现了一个简单的一维卡尔曼滤波器，
 * 用于估计一个常数状态在带噪测量中的真实值。
 *
 * 系统模型假设：
 *   状态转移： x_k = x_{k-1} (A = 1)
 *   测量模型： z_k = x_k + v_k (H = 1)，其中 v_k 为测量噪声
 *
 * 卡尔曼滤波公式：
 *   预测：  x_k^- = x_{k-1}
 *          P_k^- = P_{k-1} + Q
 *   更新：  K_k = P_k^- / (P_k^- + R)
 *          x_k = x_k^- + K_k * (z_k - x_k^-)
 *          P_k = (1 - K_k) * P_k^-
 *
 * 编译方法： gcc -o kalman kalman.c -lm
 * 运行方法： ./kalman
 ********************************************************************/

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

// 定义卡尔曼滤波器数据结构
typedef struct {
    double x;   // 状态估计
    double P;   // 估计误差协方差
    double Q;   // 过程噪声协方差
    double R;   // 测量噪声协方差
    double K;   // 卡尔曼增益
} KalmanFilter;

/*
 * 函数：KalmanFilter_Init
 * 功能：初始化卡尔曼滤波器
 * 参数：
 *      kf      - 指向 KalmanFilter 结构体的指针
 *      init_x  - 初始状态估计
 *      init_P  - 初始估计误差协方差
 *      Q       - 过程噪声协方差
 *      R       - 测量噪声协方差
 */
void KalmanFilter_Init(KalmanFilter *kf, double init_x, double init_P, double Q, double R) {
    kf->x = init_x;
    kf->P = init_P;
    kf->Q = Q;
    kf->R = R;
    kf->K = 0.0;
}

/*
 * 函数：KalmanFilter_Update
 * 功能：使用卡尔曼滤波器对新的测量值进行更新
 * 参数：
 *      kf - 指向 KalmanFilter 结构体的指针
 *      z  - 新的测量值
 * 返回值：
 *      更新后的状态估计
 *
 * 实现步骤：
 *   1. 预测： x_prior = x,  P_prior = P + Q
 *   2. 计算卡尔曼增益： K = P_prior / (P_prior + R)
 *   3. 更新状态估计： x = x_prior + K * (z - x_prior)
 *   4. 更新估计误差协方差： P = (1 - K) * P_prior
 */
double KalmanFilter_Update(KalmanFilter *kf, double z) {
    // 预测步骤
    double x_prior = kf->x;
    double P_prior = kf->P + kf->Q;
    
    // 更新步骤
    kf->K = P_prior / (P_prior + kf->R);
    kf->x = x_prior + kf->K * (z - x_prior);
    kf->P = (1 - kf->K) * P_prior;
    
    return kf->x;
}

/*
 * 函数：main
 * 功能：程序入口，演示一维卡尔曼滤波器的使用
 *
 * 实现步骤：
 *   1. 初始化卡尔曼滤波器参数
 *   2. 模拟一系列带噪测量值
 *   3. 依次调用 KalmanFilter_Update() 对测量值进行滤波更新
 *   4. 打印每一步的测量值和滤波后的状态估计
 */
int main() {
    // 设置卡尔曼滤波器参数
    double init_x = 0.0;     // 初始状态估计
    double init_P = 1.0;     // 初始估计误差协方差
    double Q = 0.001;        // 过程噪声协方差
    double R = 0.1;          // 测量噪声协方差
    
    KalmanFilter kf;
    KalmanFilter_Init(&kf, init_x, init_P, Q, R);
    
    // 模拟一系列带噪测量值（假设真实状态为 10）
    double measurements[] = {10.2, 9.8, 10.5, 10.0, 9.7, 10.3, 10.1, 9.9, 10.4, 10.0};
    int num_measurements = sizeof(measurements) / sizeof(measurements[0]);
    
    printf("卡尔曼滤波器初始状态: x = %.3f, P = %.3f\n\n", kf.x, kf.P);
    printf("测量值\t滤波后估计值\t卡尔曼增益\t估计误差协方差\n");
    
    for (int i = 0; i < num_measurements; i++) {
        double z = measurements[i];
        double x_est = KalmanFilter_Update(&kf, z);
        printf("%.3f\t%.3f\t\t%.3f\t\t%.3f\n", z, x_est, kf.K, kf.P);
    }
    
    return 0;
}

四、代码解读（方法作用解析）

1. `KalmanFilter_Init()` — 初始化卡尔曼滤波器

主要功能：
初始化卡尔曼滤波器结构体，设置初始状态估计 x、初始误差协方差 P 以及过程噪声协方差 Q 和测量噪声协方差 R。
实现细节：
将传入参数赋值给滤波器结构体的各个成员，并将卡尔曼增益 K 初始化为 0。

2. `KalmanFilter_Update()` — 卡尔曼滤波更新函数

主要功能：
对于每次新的测量值 zzz，根据卡尔曼滤波算法执行预测和更新步骤，更新状态估计 x 及估计误差协方差 P，同时计算卡尔曼增益 K。

3. `main()` — 主函数

主要功能：
模拟一系列带噪测量值，并使用卡尔曼滤波器对这些测量值进行状态估计更新。
实现细节：
- 初始化卡尔曼滤波器参数（初始状态、初始误差协方差、过程噪声和测量噪声）。
- 定义一组测量值数组（例如模拟真实状态为 10 的场景，但每个测量值带有噪声）。
- 循环调用 KalmanFilter_Update() 对每个测量值进行更新，并打印出每一步的测量值、滤波后的估计值、卡尔曼增益以及更新后的误差协方差，便于观察滤波器收敛过程。