提交更新

src/BaseTracker/BaseTrack.h

 #pragma once
 
 #include <eigen3/Eigen/Dense>
-#include "kalman_filter.h"
+#include "kalman_filter_f.h"
 #include <vector>
 #include <memory>
 
@@ -63,5 +63,5 @@ public:
     int m_updateValidCount = 3;//更新多少次认为是一个有效数据。
 
 
-    std::shared_ptr<mytracker::KalmanFilter> kf_ = nullptr;
+    std::shared_ptr<mytracker_f::KalmanFilter> kf_ = nullptr;
 };

src/BaseTracker/Track2D.cpp

@@ -3,7 +3,7 @@
 
 Track2D::Track2D():BaseTrack(8, 4)
 {
-    std::shared_ptr<mytracker::KalmanFilter> kf = std::make_shared<mytracker::KalmanFilter>(8, 4);
+    std::shared_ptr<mytracker_f::KalmanFilter> kf = std::make_shared<mytracker::KalmanFilter>(8, 4);
     kf->F_ <<
         1, 0, 0, 0, 1, 0, 0, 0,
         0, 1, 0, 0, 0, 1, 0, 0,

src/BaseTracker/kalman_filter_f.cpp

+#include "kalman_filter_f.h"
+
+namespace mytracker_f {
+
+KalmanFilter::KalmanFilter(unsigned int num_states, unsigned int num_obs) :
+        num_states_(num_states), num_obs_(num_obs) {
+    /*** Predict ***/
+    // State vector
+    x_ = Eigen::VectorXf::Zero(num_states);
+    // Predicted(a prior) state vector
+    x_predict_ = Eigen::VectorXf::Zero(num_states);
+
+    // State transition matrix F_
+    F_ = Eigen::MatrixXf::Zero(num_states, num_states);
+
+    // Error covariance matrix P
+    P_ = Eigen::MatrixXf::Zero(num_states, num_states);
+    // Predicted(a prior) error covariance matrix
+    P_predict_ = Eigen::MatrixXf::Zero(num_states, num_states);
+
+    // Covariance matrix of process noise
+    Q_ = Eigen::MatrixXf::Zero(num_states, num_states);
+
+    /*** Update ***/
+    // Observation matrix
+    H_ = Eigen::MatrixXf::Zero(num_obs, num_states);
+
+    // Covariance matrix of observation noise
+    R_ = Eigen::MatrixXf::Zero(num_obs, num_obs);
+
+    log_likelihood_delta_ = 0.0;
+    NIS_ = 0.0;
+}
+
+
+void KalmanFilter::Coast() {
+    x_predict_ = F_ * x_;
+    P_predict_ = F_ * P_ * F_.transpose() + Q_;
+}
+
+
+void KalmanFilter::Predict() {
+    Coast();
+    x_ = x_predict_;
+    P_ = P_predict_;
+}
+
+
+Eigen::VectorXf KalmanFilter::PredictionToObservation(const Eigen::VectorXf &state) {
+    return (H_*state);
+}
+
+
+void KalmanFilter::Update(const Eigen::VectorXf& z) {
+    Eigen::VectorXf z_predict = PredictionToObservation(x_predict_);
+
+    // y - innovation, z - real observation, z_predict - predicted observation
+    Eigen::VectorXf y = z - z_predict;
+
+    Eigen::MatrixXf Ht = H_.transpose();
+
+    // S - innovation covariance
+    Eigen::MatrixXf S = H_ * P_predict_ * Ht + R_;
+
+    NIS_ = y.transpose() * S.inverse() * y;
+
+//    std::cout << std::endl;
+//    std::cout << "P_predict = " << std::endl;
+//    std::cout << P_predict_ << std::endl;
+//
+//
+//    std::cout << "Z = " << std::endl;
+//    std::cout << z << std::endl;
+//
+//    std::cout << "Z_pred = " << std::endl;
+//    std::cout << z_predict << std::endl;
+//
+//    std::cout << "y = " << std::endl;
+//    std::cout << y << std::endl;
+//
+//    std::cout << "S = " << std::endl;
+//    std::cout << S << std::endl;
+//
+//    std::cout << "NIS = " << NIS_ << std::endl;
+
+
+    // K - Kalman gain
+    Eigen::MatrixXf K = P_predict_ * Ht * S.inverse();
+
+    // Updated state estimation
+    x_ = x_predict_ + K * y;
+
+    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(num_states_, num_states_);
+    // Joseph form
+    //P_ = (I - K * H_) * P_predict_ * (I - K * H_).transpose() + K * R_ * K.transpose();
+    // Optimal gain
+    P_ = (I - K * H_) * P_predict_;
+}
+
+
+float KalmanFilter::CalculateLogLikelihood(const Eigen::VectorXf& y, const Eigen::MatrixXf& S) {
+    float log_likelihood;
+
+    // Note: Computing log(M.determinant()) in Eigen C++ is risky for large matrices since it may overflow or underflow.
+    // compute the Cholesky decomposition of the innovation covariance matrix, because it is symmetric
+    // S = L * L^T = U^T * U
+    // then retrieve factor L in the decomposition
+    auto& L = S.llt().matrixL();
+
+    // find log determinant of innovation covariance matrix
+    float log_determinant = 0;
+    for (unsigned int i = 0; i < S.rows(); i++)
+        log_determinant += log(L(i, i));
+    log_determinant *= 2;
+
+    // log-likelihood expression for current iteration
+    log_likelihood = -0.5 * (y.transpose() * S.inverse() * y + num_obs_ * log(2 * M_PI) + log_determinant);
+
+    if (std::isnan(log_likelihood)) {
+    	log_likelihood = -1e50;
+	}
+
+	return log_likelihood;
+}
+
+}
\ No newline at end of file

src/BaseTracker/kalman_filter_f.h

+#pragma once
+
+#include <iostream>
+#include <eigen3/Eigen/Dense>
+
+namespace mytracker_f {
+
+// abstract class for Kalman filter
+// implementation could be KF/EKF/UKF...
+class KalmanFilter {
+public:
+    /**
+     * user need to define H matrix & R matrix
+     * @param num_states
+     * @param num_obs
+     */
+    // constructor
+    explicit KalmanFilter(unsigned int num_states, unsigned int num_obs);
+
+    // destructor
+    virtual ~KalmanFilter() = default;
+
+    /**
+     * Coast state and state covariance using the process model
+     * User can use this function without change the internal
+     * tracking state x_
+     */
+    virtual void Coast();
+
+    /**
+     * Predict without measurement update
+     */
+    void Predict();
+
+    /**
+     * This function maps the true state space into the observed space
+     * using the observation model
+     * User can implement their own method for more complicated models
+     */
+    virtual Eigen::VectorXf PredictionToObservation(const Eigen::VectorXf &state);
+
+    /**
+     * Updates the state by using Extended Kalman Filter equations
+     * @param z The measurement at k+1
+     */
+    virtual void Update(const Eigen::VectorXf &z);
+
+    /**
+     * Calculate marginal log-likelihood to evaluate different parameter choices
+     */
+    float CalculateLogLikelihood(const Eigen::VectorXf& y, const Eigen::MatrixXf& S);
+
+    // State vector
+    Eigen::VectorXf x_, x_predict_;
+
+    // Error covariance matrix
+    Eigen::MatrixXf P_, P_predict_;
+
+    // State transition matrix
+    Eigen::MatrixXf F_;
+
+    // Covariance matrix of process noise
+    Eigen::MatrixXf Q_;
+
+    // measurement matrix
+    Eigen::MatrixXf H_;
+
+    // covariance matrix of observation noise
+    Eigen::MatrixXf R_;
+
+    unsigned int num_states_, num_obs_;
+
+    float log_likelihood_delta_;
+
+    float NIS_;
+};
+}
\ No newline at end of file