extendedKalmanFilter.cpp

Go to the documentation of this file.

#include <dlrComputerVision/extendedKalmanFilter.h>
#include <dlrLinearAlgebra/linearAlgebra.h>
#include <dlrNumeric/utilities.h>

namespace dlr {

  namespace computerVision {

    // Default constructor.
    ExtendedKalmanFilter::
    ExtendedKalmanFilter(double startTime)
      : m_covariance(),
        m_state(),
        m_previousTimestamp(startTime),
        m_timestamp(startTime)
    {
      // Empty.
    }

    
    // Use this member function tell the filter about a new
    // measurement, and to request that the state estimate be
    // updated to reflect this new measurement.
    void
    ExtendedKalmanFilter::
    addMeasurement(unsigned int measurementID,
                   double timestamp,
                   numeric::Array1D<double> const& measurement,
                   numeric::Array1D<double> const& controlInput)
    {
      this->doPredictionStep(timestamp, controlInput);
      this->doMeasurementUpdate(measurementID, measurement);
    }


    void
    ExtendedKalmanFilter::
    checkMeasurementJacobians(unsigned int measurementID,
                              numeric::Array1D<double> const& epsilonArray,
                              numeric::Array2D<double>& residualArray0)
    {
      // Check arguments.
      if(epsilonArray.size() != m_state.size()) {
        DLR_THROW(common::ValueException,
                  "ExtendedKalmanFilter::checkProcessJacobians()",
                  "Argument epsilon has incorrect size.");
      }
      
      // Get the symbolically computed Jacobians and check their sizes.
      numeric::Array2D<double> measurementJacobian0;
      numeric::Array2D<double> measurementJacobian1;
      this->getMeasurementJacobians(
        measurementID, m_timestamp, m_previousTimestamp, m_state,
        measurementJacobian0, measurementJacobian1);


      numeric::Array1D<double> measurement =
        this->applyMeasurementModel(
          measurementID, m_timestamp, m_previousTimestamp, m_state);
      numeric::Array2D<double> measurementNoiseCovariance = 
        this->getMeasurementNoiseCovariance(
          measurementID, m_timestamp, m_previousTimestamp);
      if(measurementJacobian0.rows() != measurement.size()
         || measurementJacobian0.columns() != m_state.size()
         || measurementJacobian1.rows() != measurement.size()
         || (measurementJacobian1.columns()
             != measurementNoiseCovariance.rows())) {
        DLR_THROW(common::LogicException,
                  "ExtendedKalmanFilter::checkMeasurementJacobians()",
                  "At least one of the returned Jacobians has incorrect size.");
      }

      // Generate finite-difference approximation to measurementJacobian0, the 
      // Jacobian wrt state.
      numeric::Array2D<double> approximateJacobian0(
        measurementJacobian0.rows(), measurementJacobian0.columns());
      for(unsigned int columnIndex = 0; columnIndex < m_state.size();
          ++columnIndex) {
        numeric::Array1D<double> state = m_state.copy();

        state[columnIndex] -= epsilonArray[columnIndex];
        numeric::Array1D<double> resultMeasurement0 =
          this->applyMeasurementModel(
            measurementID, m_timestamp, m_previousTimestamp, state);

        state[columnIndex] += 2.0 * epsilonArray[columnIndex];
        numeric::Array1D<double> resultMeasurement1 = 
          this->applyMeasurementModel(
            measurementID, m_timestamp, m_previousTimestamp, state);

        numeric::Array1D<double> approximateGradient =
          ((resultMeasurement1 - resultMeasurement0)
           / (2.0 * epsilonArray[columnIndex]));
        for(unsigned int rowIndex = 0; rowIndex < resultMeasurement0.size();
            ++rowIndex) {
          approximateJacobian0(rowIndex, columnIndex) =
            approximateGradient[rowIndex];
        }
      }

      // Unfortunately, we don't have a good way to check the Jacobian
      // wrt noise right now.  On the bright side, it's often the
      // identity matrix, which is hard to get wrong.  In cases where
      // it's not the identity matrix, we'll trust the user to write
      // her own test.
      
      residualArray0 = approximateJacobian0 - measurementJacobian0;
    }
      
      
    void
    ExtendedKalmanFilter::
    checkProcessJacobians(numeric::Array1D<double> const& epsilonArray,
                          numeric::Array1D<double> const& controlInput,
                          numeric::Array2D<double>& residualArray0)
    {
      // Check arguments.
      if(epsilonArray.size() != m_state.size()) {
        DLR_THROW(common::ValueException,
                  "ExtendedKalmanFilter::checkProcessJacobians()",
                  "Argument epsilon has incorrect size.");
      }

      // Get the symbolically computed Jacobians and check their sizes.
      numeric::Array2D<double> processJacobian0;
      numeric::Array2D<double> processJacobian1;
      this->getProcessJacobians(
        m_timestamp, m_previousTimestamp, m_state,
        processJacobian0, processJacobian1);

      numeric::Array2D<double> processNoiseCovariance = 
        this->getProcessNoiseCovariance(m_timestamp, m_previousTimestamp);
      if(processJacobian0.rows() != m_state.size()
         || processJacobian0.columns() != m_state.size()
         || processJacobian1.rows() != m_state.size()
         || processJacobian1.columns() != processNoiseCovariance.rows()) {
        DLR_THROW(common::LogicException,
                  "ExtendedKalmanFilter::checkProcessJacobians()",
                  "At least one of the returned Jacobians has incorrect size.");
      }

      // Generate finite-difference approximation to processJacobian0, the 
      // Jacobian wrt state.
      numeric::Array2D<double> approximateJacobian0(
        processJacobian0.rows(), processJacobian0.columns());
      for(unsigned int columnIndex = 0; columnIndex < m_state.size();
          ++columnIndex) {
        numeric::Array1D<double> state = m_state.copy();

        state[columnIndex] -= epsilonArray[columnIndex];
        numeric::Array1D<double> resultState0 = 
          this->applyProcessModel(m_timestamp, m_previousTimestamp, state,
                                  controlInput);

        state[columnIndex] += 2.0 * epsilonArray[columnIndex];
        numeric::Array1D<double> resultState1 = 
          this->applyProcessModel(m_timestamp, m_previousTimestamp, state,
                                  controlInput);

        numeric::Array1D<double> approximateGradient =
          (resultState1 - resultState0) / (2.0 * epsilonArray[columnIndex]);
        for(unsigned int rowIndex = 0; rowIndex < m_state.size(); ++rowIndex) {
          approximateJacobian0(rowIndex, columnIndex) =
            approximateGradient[rowIndex];
        }
      }

      // Unfortunately, we don't have a good way to check the Jacobian
      // wrt noise right now.  On the bright side, it's often the
      // identity matrix, which is hard to get wrong.  In cases where
      // it's not the identity matrix, we'll trust the user to write
      // her own test.
      
      residualArray0 = approximateJacobian0 - processJacobian0;
    }

    
    // This member function may optionally be called to disable
    // updates of Kalman gain and estimation error covariance.
    void
    ExtendedKalmanFilter::
    freezeKalmanGain()
    {
      DLR_THROW(common::NotImplementedException,
                "ExtendedKalmanFilter::freezeKalmanGain()",
                "Sorry. Should be easy to implement, though.");
    }

    
    // This member function returns the current state estimate for
    // the filter, as well as the estimated covariance of the state
    // estimate.
    void
    ExtendedKalmanFilter::
    getStateEstimate(double& timestamp,
                     numeric::Array1D<double>& state,
                     numeric::Array2D<double>& covariance)
    {
      timestamp = m_timestamp;
      state = m_state.copy();
      covariance = m_covariance.copy();
    }

      
    // This member function sets the initial state estimate for the
    // filter, as well as the covariance of any Gaussian noise
    // reflected in the initial state estimate.
    void
    ExtendedKalmanFilter::
    setStateEstimate(double timestamp,
                     numeric::Array1D<double> const& state,
                     numeric::Array2D<double> const& covariance)
    {
      m_timestamp = timestamp;
      m_state = state.copy();
      m_covariance = covariance.copy();
    }

      
    // This member function may optionally be called to reverse the
    // effect of freezeKalmanGain().
    void
    ExtendedKalmanFilter::
    unfreezeKalmanGain()
    {
      DLR_THROW(common::NotImplementedException,
                "ExtendedKalmanFilter::unfreezeKalmanGain()",
                "Sorry. Should be easy to implement, though.");
    }
      

    void
    ExtendedKalmanFilter::
    doPredictionStep(double currentTime,
                     Array1D<double> const& controlInput)
    {
      // Prediction step is as follows:
      //
      //   xpr_k = f(xpo_(k-1) + u_(k-1), 0)
      //
      // where xpr_k is the a priori (before adding in measurements
      // taken at time k) estimate of x at time k.  It will be updated
      // by doMeasurementUpdate() to generate the a posteriori
      // estimate at time k, which reflects any new sensor data.
      // xpo_(k-1) is the a posteriori estimate of x at time (k - 1),
      // u_(k-1) is the control input at time (k - 1), f() is the user
      // supplied processmodel (which can be nonlinear), and the final
      // argument of zero reflects that the prediction step assumes no
      // noise.
      //
      // Prediction step updates state covariance estimate as follows:
      //
      //   Ppr_k = A_k * Ppo_(k-1) * (A_k)^T + W_k * Q_(k-1) * (W_k)^T
      //
      // Where Ppr_k is the a priori estimate of the covariance of our
      // state estimate at time k, Ppo_(k-1) is the a posteriori
      // estimate of the covariance at time (k-1), Q_(k-1) is the
      // covariance of our process noise at time (k - 1), A_k and W_k
      // are the process Jacobians at time k, and the right
      // superscript ^T indicates matrix transpose.
      m_previousTimestamp = m_timestamp;
      m_timestamp = currentTime;
      m_state = this->applyProcessModel(
        m_timestamp, m_previousTimestamp, m_state, controlInput);
      
      // Note(xxx): Inefficient to transpose every time.
      Array2D<double> processJacobian0;
      Array2D<double> processJacobian1;
      this->getProcessJacobians(
        m_timestamp, m_previousTimestamp, m_state,
        processJacobian0, processJacobian1);
      m_covariance = 
        matrixMultiply(
          matrixMultiply(processJacobian0, m_covariance),
          processJacobian0.transpose());
      m_covariance += 
        matrixMultiply(
          matrixMultiply(
            processJacobian1,
            this->getProcessNoiseCovariance(m_timestamp, m_previousTimestamp)),
          processJacobian1.transpose());
    }


    void
    ExtendedKalmanFilter::
    doMeasurementUpdate(unsigned int measurementID,
                        numeric::Array1D<double> const& measurement)
    {
      // Measurement step is as follows (assuming prediction step has
      // already brought us to the time of the sensor observation):
      //
      //   K_k = Ppr_k * (H_k)^T * (H_k * Ppr_k * (H_k)^T
      //                            + V_k * R * (V_k)^T)^(-1)
      //
      //   xpo_k = xpr_k + K_k * (z_k - H * xpr_k)
      //
      //   Ppo_k = (1 - K_k * H)Ppr_k
      //
      // Where K_k is the "Kalman gain" at time k, Ppr_k is the a
      // priori estimate of the covariance of our state estimate at
      // time k, H^T is the transpose of measurement matrix H, R is
      // the covariance of the measurement noise, xpr_k is the a
      // priori (before adding in measurements taken at time k)
      // estimate of x at time k, xpo_k is the a posteriori (after
      // accounting for measurements) estimate of x at time k, z_k is
      // the measurement at time k, and Ppo_k is the a posteriori
      // estimate of the covariance of our state estimate at time k.

      Array2D<double> measurementJacobian0;
      Array2D<double> measurementJacobian1;
      this->getMeasurementJacobians(
        measurementID, m_timestamp, m_previousTimestamp, m_state,
        measurementJacobian0, measurementJacobian1);

      // Convenient matrices for constructing Kalman gain, etc.
      Array2D<double> const& HMatrix = measurementJacobian0;
      Array2D<double> HTranspose = HMatrix.transpose();
      Array2D<double> const& VMatrix = measurementJacobian1;
      Array2D<double> VTranspose = VMatrix.transpose();

      // "Denominator" should be (H_k * Ppr_k * (H_k)^T) + (V_k * R_k * (V_k)^T)
      numeric::Array2D<double> denominator = 
        matrixMultiply(matrixMultiply(HMatrix, m_covariance), HTranspose);
      denominator +=
        (matrixMultiply(
          matrixMultiply(
            VMatrix,
            this->getMeasurementNoiseCovariance(
              measurementID, m_timestamp, m_previousTimestamp)),
          VTranspose));

      numeric::Array1D<double> innovation =
        measurement - this->applyMeasurementModel(
          measurementID, m_timestamp, m_previousTimestamp, m_state);
      
      numeric::Array2D<double> kalmanGain = 
        matrixMultiply(matrixMultiply(m_covariance, HTranspose),
                       linearAlgebra::inverse(denominator));

      m_state += matrixMultiply(kalmanGain, innovation);

      // Compute P_k = (I - K * H) * P_(k-1)
      numeric::Array2D<double> kTimesHMinusI =
        matrixMultiply(kalmanGain, HMatrix);
      if(kTimesHMinusI.rows() != kTimesHMinusI.columns()) {
        DLR_THROW(common::LogicException,
                  "ExtendedKalmanFilter::doMeasurementUpdate()",
                  "Programming error. K * H must be square.");
      }
      for(unsigned int ii = 0; ii < kTimesHMinusI.rows(); ++ii) {
        kTimesHMinusI(ii, ii) -= 1.0;
      }
      m_covariance = matrixMultiply(kTimesHMinusI, m_covariance);
      m_covariance *= -1.0;
    }
    
  } // namespace computerVision
  
} // namespace dlr