kalman_filter.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <sstream>
#include <string>

#include "Eigen/Dense"

#include "cyber/common/log.h"
#include "modules/common/math/matrix_operations.h"

namespace apollo {
namespace common {
namespace math {

template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
class KalmanFilter {
 public:
  KalmanFilter() {
    F_.setIdentity();
    Q_.setZero();
    H_.setIdentity();
    R_.setZero();
    B_.setZero();

    x_.setZero();
    P_.setZero();
    y_.setZero();
    S_.setZero();
    K_.setZero();
  }

  void SetStateEstimate(const Eigen::Matrix<T, XN, 1> &x,
                        const Eigen::Matrix<T, XN, XN> &P) {
    x_ = x;
    P_ = P;
    is_initialized_ = true;
  }

  KalmanFilter(const Eigen::Matrix<T, XN, 1> &x,
               const Eigen::Matrix<T, XN, XN> &P)
      : KalmanFilter() {
    SetStateEstimate(x, P);
  }

  virtual ~KalmanFilter() {}

  void SetTransitionMatrix(const Eigen::Matrix<T, XN, XN> &F) { F_ = F; }

  void SetTransitionNoise(const Eigen::Matrix<T, XN, XN> &Q) { Q_ = Q; }

  void SetObservationMatrix(const Eigen::Matrix<T, ZN, XN> &H) { H_ = H; }

  void SetObservationNoise(const Eigen::Matrix<T, ZN, ZN> &R) { R_ = R; }

  void SetStateCovariance(const Eigen::Matrix<T, XN, XN> &P) { P_ = P; }

  void SetControlMatrix(const Eigen::Matrix<T, XN, UN> &B) { B_ = B; }

  const Eigen::Matrix<T, XN, XN> &GetTransitionMatrix() const { return F_; }

  const Eigen::Matrix<T, XN, XN> &GetTransitionNoise() const { return Q_; }

  const Eigen::Matrix<T, ZN, XN> &GetObservationMatrix() const { return H_; }

  const Eigen::Matrix<T, ZN, ZN> &GetObservationNoise() const { return R_; }

  const Eigen::Matrix<T, XN, UN> &GetControlMatrix() const { return B_; }

  void Predict(
      const Eigen::Matrix<T, UN, 1> &u = Eigen::Matrix<T, UN, 1>::Zero());

  void Correct(const Eigen::Matrix<T, ZN, 1> &z);

  Eigen::Matrix<T, XN, 1> GetStateEstimate() const { return x_; }

  Eigen::Matrix<T, XN, XN> GetStateCovariance() const { return P_; }

  std::string DebugString() const;

  bool IsInitialized() const { return is_initialized_; }

 private:
  // Mean of current state belief distribution
  Eigen::Matrix<T, XN, 1> x_;

  // Covariance of current state belief dist
  Eigen::Matrix<T, XN, XN> P_;

  // State transition matrix under zero control
  Eigen::Matrix<T, XN, XN> F_;

  // Covariance of the state transition noise
  Eigen::Matrix<T, XN, XN> Q_;

  // Observation matrix
  Eigen::Matrix<T, ZN, XN> H_;

  // Covariance of observation noise
  Eigen::Matrix<T, ZN, ZN> R_;

  // Control matrix in state transition rule
  Eigen::Matrix<T, XN, UN> B_;

  // Innovation; marked as member to prevent memory re-allocation.
  Eigen::Matrix<T, ZN, 1> y_;

  // Innovation covariance; marked as member to prevent memory re-allocation.
  Eigen::Matrix<T, ZN, ZN> S_;

  // Kalman gain; marked as member to prevent memory re-allocation.
  Eigen::Matrix<T, XN, ZN> K_;

  // true iff SetStateEstimate has been called.
  bool is_initialized_ = false;
};

template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline void KalmanFilter<T, XN, ZN, UN>::Predict(
    const Eigen::Matrix<T, UN, 1> &u) {
  ACHECK(is_initialized_);

  x_ = F_ * x_ + B_ * u;

  P_ = F_ * P_ * F_.transpose() + Q_;
}

template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline void KalmanFilter<T, XN, ZN, UN>::Correct(
    const Eigen::Matrix<T, ZN, 1> &z) {
  ACHECK(is_initialized_);
  y_ = z - H_ * x_;

  S_ = static_cast<Eigen::Matrix<T, ZN, ZN>>(H_ * P_ * H_.transpose() + R_);

  K_ = static_cast<Eigen::Matrix<T, XN, ZN>>(P_ * H_.transpose() *
                                             PseudoInverse<T, ZN>(S_));

  x_ = x_ + K_ * y_;

  P_ = static_cast<Eigen::Matrix<T, XN, XN>>(
      (Eigen::Matrix<T, XN, XN>::Identity() - K_ * H_) * P_);
}

template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
inline std::string KalmanFilter<T, XN, ZN, UN>::DebugString() const {
  Eigen::IOFormat clean_fmt(4, 0, ", ", " ", "[", "]");
  std::stringstream ss;
  ss << "F = " << F_.format(clean_fmt) << "\n"
     << "B = " << B_.format(clean_fmt) << "\n"
     << "H = " << H_.format(clean_fmt) << "\n"
     << "Q = " << Q_.format(clean_fmt) << "\n"
     << "R = " << R_.format(clean_fmt) << "\n"
     << "x = " << x_.format(clean_fmt) << "\n"
     << "P = " << P_.format(clean_fmt) << "\n";
  return ss.str();
}

}  // namespace math
}  // namespace common
}  // namespace apollo