惯导kalman滤波c代码

以下是一个简单的惯导Kalman滤波的C语言代码实现，仅供参考：

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

#define PI 3.14159265

// 常数定义
const float Q = 0.001;  // 系统噪声协方差
const float R = 0.1;    // 测量噪声协方差
const float dt = 0.01;  // 采样时间间隔

// 定义Kalman滤波器结构体
typedef struct {
    float x_hat;  // 状态估计值
    float P;      // 估计误差协方差
    float Q;      // 系统噪声协方差
    float R;      // 测量噪声协方差
    float K;      // Kalman增益
} KalmanFilter;

// 初始化Kalman滤波器
void initKalmanFilter(KalmanFilter *kf, float x_init, float P_init) {
    kf->x_hat = x_init;
    kf->P = P_init;
    kf->Q = Q;
    kf->R = R;
    kf->K = 0.0;
}

// 更新Kalman滤波器
void updateKalmanFilter(KalmanFilter *kf, float z) {
    // 预测
    float x_hat_minus = kf->x_hat;
    float P_minus = kf->P + kf->Q;

    // 更新
    kf->K = P_minus / (P_minus + kf->R);
    kf->x_hat = x_hat_minus + kf->K * (z - x_hat_minus);
    kf->P = (1 - kf->K) * P_minus;
}

int main() {
    // 初始化Kalman滤波器
    KalmanFilter kf;
    initKalmanFilter(&kf, 0.0, 1.0);

    // 模拟输入数据
    float theta = 0.0;
    float omega = 10.0;
    float alpha = 0.0;
    float x = 0.0;

    // 模拟采样
    for (int i = 0; i < 1000; i++) {
        // 计算真实值
        theta += omega * dt + 0.5 * alpha * dt * dt;
        omega += alpha * dt;

        // 添加高斯噪声
        float z = theta + 0.1 * ((float)rand() / RAND_MAX - 0.5);

        // 更新Kalman滤波器
        updateKalmanFilter(&kf, z);

        // 输出结果
        printf("%f, %f, %f, %f\n", theta, omega, alpha, kf.x_hat);
    }

    return 0;
}

上述代码实现了一个简单的惯导Kalman滤波器，包括初始化、更新和输出结果等功能。程序中模拟了一个简单的物理系统，每个时间步长计算出真实值，并添加高斯噪声，然后用Kalman滤波器进行估计和校正，最后输出估计值。