用C语言表达卡尔曼滤波算法
时间: 2023-07-19 21:07:09 浏览: 59
以下是一个简单的 C 语言实现卡尔曼滤波算法的示例代码:
```c
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define Q 0.1 // 过程噪声协方差
#define R 1 // 测量噪声协方差
typedef struct {
float x; // 状态量
float p; // 状态协方差
} kalman_state;
void kalman_filter(float z, kalman_state *state) {
float x_hat = state->x; // 上一时刻估计的状态量
float p_hat = state->p + Q; // 上一时刻估计的状态协方差
// 卡尔曼增益
float k = p_hat / (p_hat + R);
// 更新状态量和状态协方差
state->x = x_hat + k * (z - x_hat);
state->p = (1 - k) * p_hat;
}
int main() {
float measurement = 1.2; // 测量值
kalman_state state = {0}; // 初始化状态
for (int i = 0; i < 10; i++) {
kalman_filter(measurement, &state);
printf("measurement = %f, state = %f\n", measurement, state.x);
// 生成模拟测量值
measurement += sin(i) * 0.5;
}
return 0;
}
```
该示例中,定义了一个 `kalman_state` 结构体,包含当前状态量 `x` 和状态协方差 `p`。`kalman_filter` 函数接受一个测量值 `z` 和一个指向状态结构体的指针,根据卡尔曼滤波算法计算并更新状态量和状态协方差。在 `main` 函数中,循环调用 `kalman_filter` 函数,并输出测量值和更新后的状态量。