/**
* Implementation of KalmanFilter class.
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/

#include <iostream>
#include <stdexcept>

#include <kalman/kalman.h>

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)
{
  I.setIdentity();
}

KalmanFilter::KalmanFilter()
  : m(0),
    n(0),
    t0(0.0),
    t(0.0),
    dt(0.0),
    initialized(false)
{
}

void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) {
  x_hat = x0;
  P = P0;
  this->t0 = t0;
  t = t0;
  initialized = true;
}

void KalmanFilter::init() {
  x_hat.setZero();
  P = P0;
  t0 = 0;
  t = t0;
  initialized = true;
}

void KalmanFilter::update(const Eigen::VectorXd& y) {

  if(!initialized)
    throw std::runtime_error("Filter is not initialized!");

  if (y.size() != m)
    throw std::runtime_error("KalmanFilter::update(): measurement vector has wrong size");

  if (A.rows() != n || A.cols() != n || C.rows() != m || C.cols() != n ||
      Q.rows() != n || Q.cols() != n || R.rows() != m || R.cols() != m ||
      P.rows() != n || P.cols() != n)
    throw std::runtime_error("KalmanFilter::update(): matrix dimensions are inconsistent");

  x_hat_new = A * x_hat;
  P = A*P*A.transpose() + Q;

  const Eigen::MatrixXd S = C * P * C.transpose() + R;
  Eigen::LDLT<Eigen::MatrixXd> ldlt(S);
  if (ldlt.info() != Eigen::Success)
    throw std::runtime_error("KalmanFilter::update(): failed to decompose innovation covariance");

  // K = P*C' * inv(S)  -> solve(S * X = (P*C')^T)
  const Eigen::MatrixXd PCt = P * C.transpose();
  const Eigen::MatrixXd KT = ldlt.solve(PCt.transpose());
  if (ldlt.info() != Eigen::Success)
    throw std::runtime_error("KalmanFilter::update(): failed to solve for Kalman gain");
  K = KT.transpose();

  x_hat_new += K * (y - C*x_hat_new);

  // Joseph form: keeps P symmetric / PSD under numeric errors
  const Eigen::MatrixXd IKC = I - K * C;
  P = IKC * P * IKC.transpose() + K * R * K.transpose();
  x_hat = x_hat_new;

  t += dt;
}

void KalmanFilter::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) {

  this->A = A;
  this->dt = dt;
  update(y);
}
