#include <stdio.h>
// 定义状态向量的维度
#define STATE_DIM 2
// 定义测量向量的维度
#define MEASURE_DIM 1
// 定义卡尔曼滤波器结构体
typedef struct {
// 状态估计向量
float x[STATE_DIM];
// 状态协方差矩阵
float P[STATE_DIM][STATE_DIM];
// 过程噪声协方差矩阵
float Q[STATE_DIM][STATE_DIM];
// 测量噪声协方差矩阵
float R[MEASURE_DIM][MEASURE_DIM];
// 状态转移矩阵
float F[STATE_DIM][STATE_DIM];
// 测量矩阵
float H[MEASURE_DIM][STATE_DIM];
} KalmanFilter;
// 初始化卡尔曼滤波器
void kalmanFilterInit(KalmanFilter *kf, float initialX, float initialP) {
// 初始化状态估计向量
kf->x[0] = initialX;
kf->x[1] = 0.0;
// 初始化状态协方差矩阵
kf->P[0][0] = initialP;
kf->P[0][1] = 0.0;
kf->P[1][0] = 0.0;
kf->P[1][1] = initialP;
// 初始化过程噪声协方差矩阵
kf->Q[0][0] = 0.001;
kf->Q[0][1] = 0.0;
kf->Q[1][0] = 0.0;
kf->Q[1][1] = 0.001;
// 初始化测量噪声协方差矩阵
kf->R[0][0] = 0.01;
// 初始化状态转移矩阵
kf->F[0][0] = 1.0;
kf->F[0][1] = 1.0;
kf->F[1][0] = 0.0;
kf->F[1][1] = 1.0;
// 初始化测量矩阵
kf->H[0][0] = 1.0;
kf->H[0][1] = 0.0;
}
// 卡尔曼滤波预测步骤
void kalmanPredict(KalmanFilter *kf, float controlInput) {
// 预测状态估计
kf->x[0] = kf->F[0][0] * kf->x[0] + kf->F[0][1] * kf->x[1] + controlInput;
// 预测状态协方差矩阵
// ...（省略具体计算）
}
// 卡尔曼滤波更新步骤
void kalmanUpdate(KalmanFilter *kf, float measurement) {
// 计算卡尔曼增益
// ...（省略具体计算）
// 更新状态估计
// ...（省略具体计算）
// 更新状态协方差矩阵
// ...（省略具体计算）
}
int main() {
// 初始化卡尔曼滤波器
KalmanFilter kf;
kalmanFilterInit(&kf, 0.0, 1.0);
// 模拟输入数据
float controlInput = 0.1;
float measurementNoise = 0.5;
// 模拟10次迭代
for (int i = 0; i < 10; ++i) {
// 预测步骤
kalmanPredict(&kf, controlInput);
// 模拟测量
float trueMeasurement = 2.0 * kf.x[0] + measurementNoise;
// 更新步骤
kalmanUpdate(&kf, trueMeasurement);
// 打印结果
printf("Iteration %d - True Value: %f, Estimated Value: %f\n", i + 1, trueMeasurement, kf.x[0]);
}
return 0;
} |
