diff options
| -rw-r--r-- | filter-kalman/kalman.cpp | 28 | ||||
| -rw-r--r-- | filter-kalman/kalman.h | 23 |
2 files changed, 28 insertions, 23 deletions
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp index 7e2050b0..804d15da 100644 --- a/filter-kalman/kalman.cpp +++ b/filter-kalman/kalman.cpp @@ -16,16 +16,14 @@ constexpr double settings::process_simga_rot; void KalmanFilter::init() { - static constexpr int NS = NUM_STATE_DOF; - static constexpr int NZ = NUM_MEASUREMENT_DOF; // allocate and initialize matrices - measurement_noise_cov = Matrix::Zero(NZ, NZ); - process_noise_cov = Matrix::Zero(NS, NS); - kalman_gain = Matrix::Zero(NS, NZ); - measurement_matrix = Matrix::Zero(NZ, NS); - state_cov = Matrix::Zero(NS, NS); - state_cov_prior = Matrix::Zero(NS, NS); - transition_matrix = Matrix::Zero(NS, NS); + measurement_noise_cov = MeasureMatrix::Zero(); + process_noise_cov = StateMatrix::Zero(); + state_cov = StateMatrix::Zero(); + state_cov_prior = StateMatrix::Zero(); + transition_matrix = StateMatrix::Zero(); + measurement_matrix = StateToMeasureMatrix::Zero(); + kalman_gain = MeasureToStateMatrix::Zero(); // initialize state variables state = StateVector::Zero(); state_prior = StateVector::Zero(); @@ -42,8 +40,8 @@ void KalmanFilter::time_update() void KalmanFilter::measurement_update(const PoseVector &measurement) { - Matrix tmp = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov; - Matrix tmp_inv = tmp.inverse(); + MeasureMatrix tmp = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov; + MeasureMatrix tmp_inv = tmp.inverse(); kalman_gain = state_cov_prior * measurement_matrix.transpose() * tmp_inv; innovation = measurement - measurement_matrix * state_prior; state = state_prior + kalman_gain * innovation; @@ -54,8 +52,8 @@ void KalmanFilter::measurement_update(const PoseVector &measurement) void KalmanProcessNoiseScaler::init() { - base_cov = Matrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF); - innovation_cov_estimate = Matrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF); + base_cov = StateMatrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF); + innovation_cov_estimate = MeasureMatrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF); } @@ -66,7 +64,7 @@ void KalmanProcessNoiseScaler::init() */ void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt) { - Matrix ddT = kf.innovation * kf.innovation.transpose(); + MeasureMatrix ddT = kf.innovation * kf.innovation.transpose(); double f = dt / (dt + settings::adaptivity_window_length); innovation_cov_estimate = f * ddT + (1. - f) * innovation_cov_estimate; @@ -114,7 +112,7 @@ void FTNoIR_Filter::fill_transition_matrix(double dt) } } -void FTNoIR_Filter::fill_process_noise_cov_matrix(Matrix &target, double dt) const +void FTNoIR_Filter::fill_process_noise_cov_matrix(StateMatrix &target, double dt) const { // This model is like movement at fixed velocity plus superimposed // brownian motion. Unlike standard models for tracking of objects diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h index 3a5488f8..a9771b8e 100644 --- a/filter-kalman/kalman.h +++ b/filter-kalman/kalman.h @@ -22,20 +22,26 @@ using namespace options; static constexpr int NUM_STATE_DOF = 12; static constexpr int NUM_MEASUREMENT_DOF = 6; -using Matrix = Eigen::MatrixXd; // variable size, heap allocated, double valued // These vectors are compile time fixed size, stack allocated +using StateToMeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_STATE_DOF>; +using StateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_STATE_DOF>; +using MeasureToStateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_MEASUREMENT_DOF>; +using MeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF>; using StateVector = Eigen::Matrix<double, NUM_STATE_DOF, 1>; using PoseVector = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, 1>; struct KalmanFilter { - Matrix - measurement_noise_cov, + MeasureMatrix + measurement_noise_cov; + StateMatrix process_noise_cov, state_cov, state_cov_prior, - kalman_gain, - transition_matrix, + transition_matrix; + MeasureToStateMatrix + kalman_gain; + StateToMeasureMatrix measurement_matrix; StateVector state, @@ -51,8 +57,9 @@ struct KalmanFilter struct KalmanProcessNoiseScaler { - Matrix - innovation_cov_estimate, + MeasureMatrix + innovation_cov_estimate; + StateMatrix base_cov; // baseline (unscaled) process noise covariance matrix void init(); void update(KalmanFilter &kf, double dt); @@ -107,7 +114,7 @@ class FTNoIR_Filter : public IFilter { PoseVector do_kalman_filter(const PoseVector &input, double dt, bool new_input); void fill_transition_matrix(double dt); - void fill_process_noise_cov_matrix(Matrix &target, double dt) const; + void fill_process_noise_cov_matrix(StateMatrix &target, double dt) const; public: FTNoIR_Filter(); void reset(); |