diff --git a/kalman-test.cpp b/kalman-test.cpp index a875ba3..d7bfa2c 100644 --- a/kalman-test.cpp +++ b/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 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; diff --git a/kalman.cpp b/kalman.cpp index eea446a..b71cd82 100644 --- a/kalman.cpp +++ b/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 #include #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); +} \ No newline at end of file diff --git a/kalman.hpp b/kalman.hpp index 7fa1525..f67ee79 100644 --- a/kalman.hpp +++ b/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 #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; };