卡尔曼滤波 C++

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

// Kalman Filter function
void KalmanFilter(const MatrixXd& F, const MatrixXd& Q, const MatrixXd& R, MatrixXd& P, VectorXd& state_mear, VectorXd& state_hat) {
    // Prediction step
    state_hat = F * state_mear;
    P = F * P * F.transpose() + Q;

    // Update step
    VectorXd y = state_mear - state_hat;
    MatrixXd H = MatrixXd::Identity(state_hat.size(), state_mear.size());
    MatrixXd S = H * P * H.transpose() + R;
    MatrixXd K = P * H.transpose() * S.inverse();
    state_hat = state_hat + K * y;
    P = (MatrixXd::Identity(state_hat.size(), state_hat.size()) - K * H) * P;
}

int main() {
    // Define initial state estimate
    VectorXd state_hat(6);
    state_hat << 0, 0, 0, 0, 0, 0;

    // Define initial state covariance matrix
    MatrixXd P(6, 6);
    P << 1, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0,
         0, 0, 1, 0, 0, 0,
         0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 1;

    // Define system dynamics matrix F
    MatrixXd F(6, 6);
    F << 1, 0, 0, 1, 0, 0,
         0, 1, 0, 0, 1, 0,
         0, 0, 1, 0, 0, 1,
         0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 1;

    // Define process noise covariance matrix Q
    MatrixXd Q(6, 6);
    Q << 0.01, 0, 0, 0, 0, 0,
         0, 0.01, 0, 0, 0, 0,
         0, 0, 0.01, 0, 0, 0,
         0, 0, 0, 0.01, 0, 0,
         0, 0, 0, 0, 0.01, 0,
         0, 0, 0, 0, 0, 0.01;

    // Define measurement noise covariance matrix R
    MatrixXd R(6, 6);
    R << 1, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0,
         0, 0, 1, 0, 0, 0,
         0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 1;

    // Define a sample measurement in the form of target vector
    VectorXd target(3);
    target << 1.2, 3.4, 5.6;

    // Map target vector to state_mear
    VectorXd state_mear(6);
    state_mear << target[0], 0, target[1], 0, target[2], 0;

    // Call Kalman Filter function
    KalmanFilter(F, Q, R, P, state_mear, state_hat);

    std::cout << "Updated state estimate:\n" << state_hat << std::endl;

    return 0;
}