用c语言写一个卡尔曼滤波函数

好的，您可以使用以下代码编写卡尔曼滤波函数：

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

#define N 2     //状态量(2)，可根据需要改变  
#define M 1     //观测量(1), 可根据需要改变

void KalmanFilter(float* z, float* x)
{
   float A[N][N] = {{1, 1}, {0, 1}};          //状态转移矩阵  
   float H[M][N] = {{1, 0}};                 //观测矩阵  
   float Q[N][N] = {{0.0001, 0}, {0, 0.0001}}; //过程噪声协方差  
   float R[M][M] = {{0.005}};                //观测噪声协方差  
   float P[N][N] = {{1, 0}, {0, 1}};          //后验误差协方差  
   float I[N][N] = {{1, 0}, {0, 1}};          //单位矩阵  
   float u[N] = {0};                         //外部状态输入为0  

   float K[N][M];                            //卡尔曼增益  
   float y[M];                               //残差  
   float S[M][M];                            //残差协方差  
   float P_temp[N][N];                       //临时矩阵  

   int i, j, k;

   for (k = 0; k < 100; k++)   //100个时刻  
   {
      //预测  
      for (i = 0; i < N; i++) {
         u[i] = 0;            //外部输入设为0  
         x[i] = 0;            //清零x  
         for (j = 0; j < N; j++)
            x[i] += A[i][j] * x[j];
         x[i] += u[i];
      }
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P_temp[i][j] = 0;
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P_temp[i][j] += A[i][k] * P[k][j];
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P[i][j] = 0;
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P[i][j] += P_temp[i][j] * A[k][j];
      for (i = 0; i < N; i++)
         for (j = 0; j < M; j++)
            y[j] = z[j] - H[j][k] * x[k];  //计算残差  

      //计算卡尔曼增益  
      for (i = 0; i < N; i++)
         for (j = 0; j < M; j++)
            S[i][j] = 0;
      for (i = 0; i < M; i++)
         for (j = 0; j < N; j++)
            S[i][j] += H[i][k] * P[k][j];
      for (i = 0; i < M; i++)
         for (j = 0; j < M; j++)
            S[i][j] += R[i][j];
      for (i = 0; i < N; i++)
         for (j = 0; j < M; j++)
            K[i][j] = 0;
      for (i = 0; i < N; i++)
         for (j = 0; j < M; j++)
            for (k = 0; k < M; k++)
               K[i][j] += P[k][i] * H[k][j] / S[k][k];

      //更新  
      for (i = 0; i < N; i++)
         x[i] += K[i][k] * y[k];
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P_temp[i][j] = 0;
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P_temp[i][j] += I[i][j] - K[i][k] * H[k][j];
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            P[i][j] = 0;
      for (i = 0; i < N; i++)
         for (j = 0; j < N; j++)
            for (k = 0; k < N; k++)
               P[i][j] += P_temp[i][k] * P[k][j];

      //输出  
      printf("%f\n", x[0]);
   }
}

int main(void)
{
   float z[1] = {0.8496}; //观测值
   float x[N] = {0.1, 0.1}; //状态值
   KalmanFilter(z, x);
   return 0;
}

这个函数可以通过KalmanFilter()函数调用，其中z是观测值，x是状态值。函数中的参数和变量名称可以自由修改。