summaryrefslogtreecommitdiffhomepage
path: root/filter-kalman/kalman.h
diff options
context:
space:
mode:
authorDaMichel <mw.pub@welter-4d.de>2016-08-02 10:58:27 +0200
committerDaMichel <mw.pub@welter-4d.de>2016-08-02 13:22:53 +0200
commit4c0f65a9ae72fe0b2277ff704cc7f46c2467d97f (patch)
treeb5890cab2ca53e80ce9532d63b5b6537fdee8d8e /filter-kalman/kalman.h
parent7dfed29631b5568a203fb1c5052748ab831476ac (diff)
filter/kalman: use fixed size matrices
Diffstat (limited to 'filter-kalman/kalman.h')
-rw-r--r--filter-kalman/kalman.h23
1 files changed, 15 insertions, 8 deletions
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();