summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--filter-kalman/kalman.cpp28
-rw-r--r--filter-kalman/kalman.h23
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();