多维卡尔曼滤波C代码

以下是一个使用C语言实现的多维卡尔曼滤波器的示例代码：

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

#define NUM_STATES 2
#define NUM_OBSERVATIONS 1

typedef struct {
    double state[NUM_STATES];
    double covariance[NUM_STATES][NUM_STATES];
} KalmanFilter;

void kalman_filter_init(KalmanFilter* kf, double initial_state[NUM_STATES], double initial_covariance[NUM_STATES][NUM_STATES]) {
    int i, j;
    for (i = 0; i < NUM_STATES; i++) {
        kf->state[i] = initial_state[i];
        for (j = 0; j < NUM_STATES; j++) {
            kf->covariance[i][j] = initial_covariance[i][j];
        }
    }
}

void kalman_filter_predict(KalmanFilter* kf, double control_input[NUM_STATES], double process_noise[NUM_STATES][NUM_STATES]) {
    int i, j;
    double predicted_state[NUM_STATES];
    double predicted_covariance[NUM_STATES][NUM_STATES];

    // Predict state
    for (i = 0; i < NUM_STATES; i++) {
        predicted_state[i] = 0;
        for (j = 0; j < NUM_STATES; j++) {
            predicted_state[i] += kf->state[j] * control_input[j];
        }
    }

    // Predict covariance
    for (i = 0; i < NUM_STATES; i++) {
        for (j = 0; j < NUM_STATES; j++) {
            predicted_covariance[i][j] = 0;
            int k;
            for (k = 0; k < NUM_STATES; k++) {
                predicted_covariance[i][j] += kf->covariance[i][k] * kf->covariance[k][j];
            }
            predicted_covariance[i][j] += process_noise[i][j];
        }
    }

    // Update state and covariance
    for (i = 0; i < NUM_STATES; i++) {
        kf->state[i] = predicted_state[i];
        for (j = 0; j < NUM_STATES; j++) {
            kf->covariance[i][j] = predicted_covariance[i][j];
        }
    }
}

void kalman_filter_update(KalmanFilter* kf, double observation[NUM_OBSERVATIONS], double observation_noise[NUM_OBSERVATIONS][NUM_OBSERVATIONS]) {
    int i, j;
    double innovation[NUM_OBSERVATIONS];
    double innovation_covariance[NUM_OBSERVATIONS][NUM_OBSERVATIONS];
    double kalman_gain[NUM_STATES][NUM_OBSERVATIONS];

    // Calculate innovation
    for (i = 0; i < NUM_OBSERVATIONS; i++) {
        innovation[i] = observation[i];
        for (j = 0; j < NUM_STATES; j++) {
            innovation[i] -= kf->state[j];
        }
    }

    // Calculate innovation covariance
    for (i = 0; i < NUM_OBSERVATIONS; i++) {
        for (j = 0; j < NUM_OBSERVATIONS; j++) {
            innovation_covariance[i][j] = kf->covariance[i][j] + observation_noise[i][j];
        }
    }

    // Calculate Kalman gain
    for (i = 0; i < NUM_STATES; i++) {
        for (j = 0; j < NUM_OBSERVATIONS; j++) {
            kalman_gain[i][j] = 0;
            int k;
            for (k = 0; k < NUM_OBSERVATIONS; k++) {
                kalman_gain[i][j] += kf->covariance[i][k] * innovation_covariance[k][j];
            }
            kalman_gain[i][j] /= innovation_covariance[j][j];
        }
    }

    // Update state and covariance
    for (i = 0; i < NUM_STATES; i++) {
        kf->state[i] += kalman_gain[i][0] * innovation[0];
        for (j = 0; j < NUM_STATES; j++) {
            kf->covariance[i][j] -= kalman_gain[i][0] * innovation_covariance[0][j] * kalman_gain[i][0];
        }
    }
}

int main() {
    // Initialize Kalman filter
    KalmanFilter kf;
    double initial_state[NUM_STATES] = {0, 0};
    double initial_covariance[NUM_STATES][NUM_STATES] = {{1, 0}, {0, 1}};
    kalman_filter_init(&kf, initial_state, initial_covariance);

    // Define control input
    double control_input[NUM_STATES] = {1, 1};

    // Define process noise
    double process_noise[NUM_STATES][NUM_STATES] = {{0.1, 0}, {0, 0.1}};

    // Define observation
    double observation[NUM_OBSERVATIONS] = {1};

    // Define observation noise
    double observation_noise[NUM_OBSERVATIONS][NUM_OBSERVATIONS] = {{0.1}};

    // Perform Kalman filter prediction and update
    kalman_filter_predict(&kf, control_input, process_noise);
    kalman_filter_update(&kf, observation, observation_noise);

    // Print updated state
    printf("Updated state: [%f, %f]\n", kf.state[0], kf.state[1]);

    return 0;
}