联邦卡尔曼滤波c语言代码

以下是一个简单的联邦卡尔曼滤波的C语言代码示例，用于融合两个传感器的测量值：

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

#define n 2 //状态量维度
#define m 2 //测量值维度

void FKalmanFilter(float z[m], float x[n], float P[n][n], float F[n][n], float H[m][n], float Q[n][n], float R[m][m])
{
    //预测
    float x_predict[n];
    for (int i = 0; i < n; i++)
    {
        x_predict[i] = 0;
        for (int j = 0; j < n; j++)
        {
            x_predict[i] += F[i][j] * x[j];
        }
    }

    float P_predict[n][n];
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            P_predict[i][j] = 0;
            for (int k = 0; k < n; k++)
            {
                P_predict[i][j] += F[i][k] * P[k][j] * F[j][k];
            }
            P_predict[i][j] += Q[i][j];
        }
    }

    //更新
    float K[n][m];
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < m; j++)
        {
            K[i][j] = 0;
            for (int k = 0; k < n; k++)
            {
                K[i][j] += P_predict[i][k] * H[j][k];
            }
            K[i][j] /= R[j][j] + H[j][0] * P_predict[0][i] * H[j][0] + H[j][1] * P_predict[1][i] * H[j][1];
        }
    }

    float x_update[n];
    for (int i = 0; i < n; i++)
    {
        x_update[i] = x_predict[i] + K[0][0] * (z[0] - H[0][0] * x_predict[0] - H[0][1] * x_predict[1]) + K[1][0] * (z[1] - H[1][0] * x_predict[0] - H[1][1] * x_predict[1]);
    }

    float P_update[n][n];
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            P_update[i][j] = P_predict[i][j] - K[i][0] * H[0][j] * P_predict[j][0] - K[i][1] * H[1][j] * P_predict[j][1];
        }
    }

    //更新状态量和协方差矩阵
    for (int i = 0; i < n; i++)
    {
        x[i] = x_update[i];
        for (int j = 0; j < n; j++)
        {
            P[i][j] = P_update[i][j];
        }
    }
}

int main()
{
    float z[m] = {10.1, 7.9}; //两个传感器的测量值
    float x[n] = {0, 0}; //状态量初始值
    float P[n][n] = {{1, 0}, {0, 1}}; //协方差矩阵初始值
    float F[n][n] = {{1, 0.1}, {0, 1}}; //状态转移矩阵
    float H[m][n] = {{1, 0}, {0, 1}}; //测量矩阵
    float Q[n][n] = {{0.01, 0}, {0, 0.01}}; //过程噪声协方差矩阵
    float R[m][m] = {{0.1, 0}, {0, 0.1}}; //测量噪声协方差矩阵

    for (int i = 0; i < 10; i++)
    {
        FKalmanFilter(z, x, P, F, H, Q, R);

        printf("第%d次迭代：\n", i + 1);
        printf("状态量：%.3f %.3f\n", x[0], x[1]);
        printf("协方差矩阵：%.3f %.3f\n%.3f %.3f\n\n", P[0][0], P[0][1], P[1][0], P[1][1]);
    }

    return 0;
}

该代码使用了一个简单的二维状态量和二维测量值，可以通过修改数组大小来适应不同的应用场景。另外，该代码仅实现了最基本的联邦卡尔曼滤波，可以根据需要进行修改和扩展。