卡尔曼滤波c语言实现

卡尔曼滤波是一种用于估计系统状态的方法，它能够通过测量和先前状态的信息来对未来状态进行预测。以下是一个简单的卡尔曼滤波的C语言实现：

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

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

void kalman_filter(float *x_hat, float *P, float *z, float *Q, float *R, float *H, float *F, float *u){
    // x_hat: 初始状态估计
    // P: 初始协方差矩阵
    // z: 观测向量
    // Q: 系统噪声协方差矩阵
    // R: 观测噪声协方差矩阵
    // H: 观测矩阵
    // F: 状态转移矩阵
    // u: 控制向量

    float x[N];     // 状态向量
    float P_pred[N][N];     // 预测协方差矩阵
    float x_pred[N];        // 预测状态向量
    float K[N][M];          // 卡尔曼增益矩阵
    float S[M][M];          // 观测协方差矩阵
    float y[M];             // 估计误差

    // 预测状态向量和预测协方差矩阵
    for(int i=0; i<N; i++){
        x_pred[i] = 0;
        for(int j=0; j<N; j++){
            x_pred[i] += F[N*i+j] * x_hat[j];
        }
        x_pred[i] += u[i];
        for(int j=0; j<N; j++){
            P_pred[i][j] = 0;
            for(int k=0; k<N; k++){
                P_pred[i][j] += F[N*i+k] * P[k*N+j] * F[N*j+k];
            }
            P_pred[i][j] += Q[i*N+j];
        }
    }

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

    // 更新状态向量和协方差矩阵
    for(int i=0; i<M; i++){
        y[i] = z[i] - H[M*i] * x_pred[0];
    }
    for(int i=0; i<N; i++){
        x[i] = x_pred[i] + K[i][0] * y[0];
        for(int j=0; j<N; j++){
            P[i*N+j] = P_pred[i][j] - K[i][0] * H[j];
        }
    }

    // 更新状态估计
    for(int i=0; i<N; i++){
        x_hat[i] = x[i];
    }

}

int main(){
    float x_hat[N] = {0, 0};     // 初始状态估计
    float P[N][N] = {{1, 0}, {0, 1}};    // 初始协方差矩阵
    float Q[N][N] = {{0.001, 0}, {0, 0.001}};   // 系统噪声协方差矩阵
    float R[M][M] = {{0.01}};    // 观测噪声协方差矩阵
    float H[M][N] = {{1, 0}};   // 观测矩阵
    float F[N][N] = {{1, 0.1}, {0, 1}};    // 状态转移矩阵
    float u[N] = {0, 0};    // 控制向量
    float z[M] = {1.1};     // 观测向量

    kalman_filter(x_hat, (float *)P, z, (float *)Q, (float *)R, (float *)H, (float *)F, u);

    printf("x_hat = [%f, %f]\n", x_hat[0], x_hat[1]);

    return 0;
}

这个代码实现了一个简单的一维卡尔曼滤波器，它使用了一些预定义的矩阵来表示系统参数。在实际应用中，这些矩阵应该根据具体的问题进行调整。