用C语言写一个卡尔曼滤波在船舶GPS的应用代码

以下是一个简单的用C语言实现卡尔曼滤波在船舶GPS的应用代码：

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

#define dt 0.1 // 时间间隔
#define sigma_a 0.1 // 加速度噪声标准差
#define sigma_z 5 // GPS测量噪声标准差

// 卡尔曼滤波参数
float x_hat[2] = {0, 0}; // 状态向量
float P[2][2] = {{1, 0}, {0, 1}}; // 协方差矩阵
float Q[2][2] = {{sigma_a*sigma_a*dt*dt/4, sigma_a*sigma_a*dt/2}, {sigma_a*sigma_a*dt/2, sigma_a*sigma_a}}; // 状态模型噪声协方差矩阵
float R = sigma_z*sigma_z; // 观测噪声协方差矩阵

// 状态模型
void stateModel(float u)
{
    float A[2][2] = {{1, dt}, {0, 1}}; // 状态转移矩阵
    float B[2] = {dt*dt/2, dt}; // 控制矩阵
    float w[2] = {sigma_a*sqrt(dt)*randn(), sigma_a*randn()}; // 状态模型噪声

    // 预测状态向量和协方差矩阵
    float x_hat_minus[2] = {0, 0};
    float P_minus[2][2] = {{0, 0}, {0, 0}};
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            x_hat_minus[i] += A[i][j]*x_hat[j];
            P_minus[i][j] = A[i][j]*P[j][j]*A[i][j] + Q[i][j];
        }
        x_hat_minus[i] += B[i]*u;
    }

    // 更新状态向量和协方差矩阵
    for (int i = 0; i < 2; i++) {
        x_hat[i] = x_hat_minus[i];
        for (int j = 0; j < 2; j++) {
            P[i][j] = P_minus[i][j];
        }
    }
}

// 观测模型
float observationModel(float z)
{
    float H[2] = {1, 0}; // 观测矩阵
    float v = z - H[0]*x_hat[0]; // 观测噪声
    float K[2] = {P[0][0]*H[0]/(H[0]*P[0][0]*H[0] + R), 0}; // 卡尔曼增益
    float x_hat_new[2] = {x_hat[0] + K[0]*v, x_hat[1]}; // 更新状态向量

    // 更新协方差矩阵
    float P_new[2][2] = {{0, 0}, {0, 0}};
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            P_new[i][j] = (1 - K[0]*H[0])*P[i][j];
        }
        P_new[i][i] += K[0]*R*K[0];
    }

    // 更新状态向量和协方差矩阵
    for (int i = 0; i < 2; i++) {
        x_hat[i] = x_hat_new[i];
        for (int j = 0; j < 2; j++) {
            P[i][j] = P_new[i][j];
        }
    }

    return x_hat[0];
}

// 生成正态分布随机数
float randn()
{
    float u1 = (float)rand()/RAND_MAX;
    float u2 = (float)rand()/RAND_MAX;
    return sqrt(-2*log(u1))*cos(2*M_PI*u2);
}

int main()
{
    // 生成模拟数据
    float x_true[100], x_obs[100];
    for (int i = 0; i < 100; i++) {
        float u = 1; // 控制输入
        x_true[i] = i + randn();
        stateModel(u);
        x_obs[i] = observationModel(x_true[i]) + sigma_z*randn();
    }

    // 输出结果
    for (int i = 0; i < 100; i++) {
        printf("%f, %f, %f\n", x_true[i], x_obs[i], x_hat[0]);
    }

    return 0;
}

这段代码实现了一个简单的船舶GPS的卡尔曼滤波应用，通过模拟数据进行了验证。需要注意的是，实际应用中需要根据具体情况进行参数的选择和调整，以达到最佳的滤波效果。