Renamed the variables used in the repo

kalman-test.cpp

@@ -18,29 +18,29 @@ int main(int argc, char* argv[]) {
 
   double dt = 1.0/30; // Time step
 
-  Eigen::MatrixXd A(n, n); // System dynamics matrix
-  Eigen::MatrixXd C(m, n); // Output matrix
-  Eigen::MatrixXd Q(n, n); // Process noise covariance
-  Eigen::MatrixXd R(m, m); // Measurement noise covariance
-  Eigen::MatrixXd P(n, n); // Estimate error covariance
+  Eigen::MatrixXd state_transition(n, n); // System dynamics matrix
+  Eigen::MatrixXd observation(m, n); // Output matrix
+  Eigen::MatrixXd process_noise_cov(n, n); // Process noise covariance
+  Eigen::MatrixXd measurement_cov(m, m); // Measurement noise covariance
+  Eigen::MatrixXd estimated_cov(n, n); // Estimate error covariance
 
   // Discrete LTI projectile motion, measuring position only
-  A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
-  C << 1, 0, 0;
+  state_transition << 1, dt, 0, 0, 1, dt, 0, 0, 1;
+  observation << 1, 0, 0;
 
   // Reasonable covariance matrices
-  Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
-  R << 5;
-  P << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
+  process_noise_cov << .05, .05, .0, .05, .05, .0, .0, .0, .0;
+  measurement_cov << 5;
+  estimated_cov << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
 
-  std::cout << "A: \n" << A << std::endl;
-  std::cout << "C: \n" << C << std::endl;
-  std::cout << "Q: \n" << Q << std::endl;
-  std::cout << "R: \n" << R << std::endl;
-  std::cout << "P: \n" << P << std::endl;
+  std::cout << "state_transition: \n" << state_transition << std::endl;
+  std::cout << "observation: \n" << observation << std::endl;
+  std::cout << "process_noise_cov: \n" << process_noise_cov << std::endl;
+  std::cout << "measurement_cov: \n" << measurement_cov << std::endl;
+  std::cout << "estimated_cov: \n" << estimated_cov << std::endl;
 
   // Construct the filter
-  KalmanFilter kf(dt,A, C, Q, R, P);
+  KalmanFilter kf(dt,state_transition, observation, process_noise_cov, measurement_cov, estimated_cov);
 
   // List of noisy position measurements (y)
   std::vector<double> measurements = {
@@ -56,21 +56,21 @@ int main(int argc, char* argv[]) {
   };
 
   // Best guess of initial states
-  Eigen::VectorXd x0(n);
+  Eigen::VectorXd initial_state(n);
   double t = 0;
-  x0 << measurements[0], 0, -9.81;
-  kf.init(t, x0);
+  initial_state << measurements[0], 0, -9.81;
+  kf.init(t, initial_state);
 
   // Feed measurements into filter, output estimated states
 
   Eigen::VectorXd y(m);
-  std::cout << "t = " << t << ", " << "x_hat[0]: " << kf.state().transpose() << std::endl;
+  std::cout << "t = " << t << ", " << "predicted_state[0]: " << kf.state().transpose() << std::endl;
   for(int i = 0; i < measurements.size(); i++) {
     t += dt;
     y << measurements[i];
     kf.update(y);
     std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.transpose()
-        << ", x_hat[" << i << "] = " << kf.state().transpose() << std::endl;
+        << ", predicted_state[" << i << "] = " << kf.state().transpose() << std::endl;
   }
 
   return 0;

kalman.cpp

@@ -1,65 +1,73 @@
 /**
-* Implementation of KalmanFilter class.
-*
-* @author: Hayk Martirosyan
-* @date: 2014.11.15
-*/
+ * Implementation of KalmanFilter class.
+ *
+ * @author: Hayk Martirosyan
+ * @date: 2014.11.15
+ */
 
 #include <iostream>
 #include <stdexcept>
 
 #include "kalman.hpp"
 
-KalmanFilter::KalmanFilter(
-    double dt,
-    const Eigen::MatrixXd& A,
-    const Eigen::MatrixXd& C,
-    const Eigen::MatrixXd& Q,
-    const Eigen::MatrixXd& R,
-    const Eigen::MatrixXd& P)
-  : A(A), C(C), Q(Q), R(R), P0(P),
-    m(C.rows()), n(A.rows()), dt(dt), initialized(false),
-    I(n, n), x_hat(n), x_hat_new(n)
+KalmanFilter::KalmanFilter(double time_step, const Eigen::MatrixXd& state_transition,
+                           const Eigen::MatrixXd& observation, const Eigen::MatrixXd& process_noise_cov,
+                           const Eigen::MatrixXd& measurement_cov, const Eigen::MatrixXd& estimated_cov)
+  : state_transition(state_transition)
+  , observation(observation)
+  , process_noise_cov(process_noise_cov)
+  , measurement_cov(measurement_cov)
+  , initial_state_covariance(estimated_cov)
+  , time_step(time_step)
+  , initialized(false)
+  , identity(state_transition.rows(), state_transition.rows())
+  , predicted_state(state_transition.rows())
+  , estimated_state(state_transition.rows())
 {
-  I.setIdentity();
+  identity.setIdentity();
 }
 
-KalmanFilter::KalmanFilter() {}
+KalmanFilter::KalmanFilter()
+{
+}
 
-void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
-  x_hat = x0;
-  P = P0;
-  this->t0 = t0;
-  t = t0;
+void KalmanFilter::init(double initial_time, const Eigen::VectorXd& initial_state)
+{
+  predicted_state = initial_state;
+  estimated_cov = initial_state_covariance;
+  this->initial_time = initial_time;
+  current_time = initial_time;
   initialized = true;
 }
 
-void KalmanFilter::init() {
-  x_hat.setZero();
-  P = P0;
-  t0 = 0;
-  t = t0;
+void KalmanFilter::init()
+{
+  predicted_state.setZero();
+  estimated_cov = initial_state_covariance;
+  initial_time = 0;
+  current_time = initial_time;
   initialized = true;
 }
 
-void KalmanFilter::update(const Eigen::VectorXd& y) {
-
-  if(!initialized)
+void KalmanFilter::update(const Eigen::VectorXd& process_noise)
+{
+  if (!initialized)
     throw std::runtime_error("Filter is not initialized!");
 
-  x_hat_new = A * x_hat;
-  P = A*P*A.transpose() + Q;
-  K = P*C.transpose()*(C*P*C.transpose() + R).inverse();
-  x_hat_new += K * (y - C*x_hat_new);
-  P = (I - K*C)*P;
-  x_hat = x_hat_new;
-
-  t += dt;
+  estimated_state = state_transition * predicted_state;
+  estimated_cov = state_transition * estimated_cov * state_transition.transpose() + process_noise_cov;  
+  kalman_gain = estimated_cov * observation.transpose() *
+                (observation * estimated_cov * observation.transpose() + measurement_cov).inverse();
+  estimated_state += kalman_gain * (process_noise - observation * estimated_state);
+  estimated_cov = (identity - kalman_gain * observation) * estimated_cov;
+  predicted_state = estimated_state;
+  current_time += time_step;
 }
 
-void KalmanFilter::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) {
-
-  this->A = A;
-  this->dt = dt;
-  update(y);
-}
+void KalmanFilter::update(const Eigen::VectorXd& process_noise, double time_step,
+                          const Eigen::MatrixXd state_transition)
+{
+  this->state_transition = state_transition;
+  this->time_step = time_step;
+  update(process_noise);
+}

kalman.hpp

@@ -1,91 +1,74 @@
 /**
-* Kalman filter implementation using Eigen. Based on the following
-* introductory paper:
-*
-*     http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
-*
-* @author: Hayk Martirosyan
-* @date: 2014.11.15
-*/
+ * Kalman filter implementation using Eigen. Based on the following
+ * introductory paper:
+ *
+ *     http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
+ *
+ * @author: Hayk Martirosyan
+ * @date: 2014.11.15
+ */
 
 #include <Eigen/Dense>
 
 #pragma once
 
-class KalmanFilter {
+class KalmanFilter
+{
 
 public:
-
-  /**
-  * Create a Kalman filter with the specified matrices.
-  *   A - System dynamics matrix
-  *   C - Output matrix
-  *   Q - Process noise covariance
-  *   R - Measurement noise covariance
-  *   P - Estimate error covariance
-  */
   KalmanFilter(
-      double dt,
-      const Eigen::MatrixXd& A,
-      const Eigen::MatrixXd& C,
-      const Eigen::MatrixXd& Q,
-      const Eigen::MatrixXd& R,
-      const Eigen::MatrixXd& P
-  );
+      double time_step,
+      const Eigen::MatrixXd &state_transition,
+      const Eigen::MatrixXd &observation,
+      const Eigen::MatrixXd &process_noise_cov,
+      const Eigen::MatrixXd &measurement_cov,
+      const Eigen::MatrixXd &estimated_cov);
 
   /**
-  * Create a blank estimator.
-  */
+   * Create a blank estimator.
+   */
   KalmanFilter();
 
   /**
-  * Initialize the filter with initial states as zero.
-  */
+   * Initialize the filter with initial states as zero.
+   */
   void init();
 
   /**
-  * Initialize the filter with a guess for initial states.
-  */
-  void init(double t0, const Eigen::VectorXd& x0);
+   * Initialize the filter with a guess for initial states.
+   */
+  void init(double initial_time, const Eigen::VectorXd &initial_state);
 
   /**
-  * Update the estimated state based on measured values. The
-  * time step is assumed to remain constant.
-  */
-  void update(const Eigen::VectorXd& y);
+   * Update the estimated state based on measured values. The
+   * time step is assumed to remain constant.
+   */
+  void update(const Eigen::VectorXd &y);
 
   /**
-  * Update the estimated state based on measured values,
-  * using the given time step and dynamics matrix.
-  */
-  void update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A);
+   * Update the estimated state based on measured values,
+   * using the given time step and dynamics matrix.
+   */
+  void update(const Eigen::VectorXd &y, double time_step, const Eigen::MatrixXd state_transition);
 
   /**
-  * Return the current state and time.
-  */
-  Eigen::VectorXd state() { return x_hat; };
-  double time() { return t; };
+   * Return the current state and time.
+   */
+  Eigen::VectorXd state() { return predicted_state; };
+  double time() { return current_time; };
 
-private:
 
-  // Matrices for computation
-  Eigen::MatrixXd A, C, Q, R, P, K, P0;
-
-  // System dimensions
-  int m, n;
+private:
+  Eigen::MatrixXd state_transition, observation, process_noise_cov,
+      measurement_cov, estimated_cov, kalman_gain, initial_state_covariance;
 
-  // Initial and current time
-  double t0, t;
+  double initial_time, current_time;
 
-  // Discrete time step
-  double dt;
+  double time_step;
 
-  // Is the filter initialized?
   bool initialized;
 
-  // n-size identity
-  Eigen::MatrixXd I;
+  Eigen::MatrixXd identity;
 
-  // Estimated states
-  Eigen::VectorXd x_hat, x_hat_new;
+  Eigen::VectorXd predicted_state, estimated_state;
 };