C语言 卡尔曼滤波算法

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。C语言实现Kalman Filter Algorithm的代码如下：

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

#define N 2 // 状态量维数
#define M 1 // 观测量维数

void Kalman_Filter(float *z,float *x_hat,float *P,float *Q,float *R,float *K)
{
    float F[N][N] = {{1,1},{0,1}}; // 状态转移矩阵
    float H[M][N] = {{1,0}}; // 观测矩阵
    float I[N][N] = {{1,0},{0,1}}; // 单位矩阵
    float P_temp1[N][N],P_temp2[N][N],K_temp[M][N];
    float x_hat_temp[N],z_temp[M];
    float Q_temp[N][N],R_temp[M][M];
    int i,j;

    // 预测
    for(i=0;i<N;i++)
    {
        x_hat_temp[i] = 0;
        for(j=0;j<N;j++)
        {
            x_hat_temp[i] += F[i][j]*x_hat[j];        }
    }
    for(i=0;i<N;i++)
    {
        for(j=0;j<N;j++)
        {
            P_temp1[i][j] = F[i][j]*P[j*N+i]*F[i][j];
        }
    }
    for(i=0;i<N;i++)
    {
        for(j=0;j<N;j++)
        {
            P_temp2[i][j] = P_temp1[i][j] + Q[i*N+j];
        }
    }

    // 更新
    for(i=0;i<M;i++)