Implements a discrete-time Kalman filter. 更多...

#include <kalman_filter.h>

apollo::common::math::KalmanFilter< T, XN, ZN, UN > 的协作图:

Public 成员函数
	KalmanFilter ()
	Constructor which defers initialization until the initial state distribution parameters are set (with SetStateEstimate), typically on the first observation

void	SetStateEstimate (const Eigen::Matrix< T, XN, 1 > &x, const Eigen::Matrix< T, XN, XN > &P)
	Sets the initial state belief distribution.

	KalmanFilter (const Eigen::Matrix< T, XN, 1 > &x, const Eigen::Matrix< T, XN, XN > &P)
	Constructor which fully initializes the Kalman filter

virtual	~KalmanFilter ()
	Destructor

void	SetTransitionMatrix (const Eigen::Matrix< T, XN, XN > &F)
	Changes the system transition function under zero control.

void	SetTransitionNoise (const Eigen::Matrix< T, XN, XN > &Q)
	Changes the covariance matrix of the transition noise.

void	SetObservationMatrix (const Eigen::Matrix< T, ZN, XN > &H)
	Changes the observation matrix, which maps states to observations.

void	SetObservationNoise (const Eigen::Matrix< T, ZN, ZN > &R)
	Changes the covariance matrix of the observation noise.

void	SetStateCovariance (const Eigen::Matrix< T, XN, XN > &P)
	Changes the covariance matrix of current state belief distribution.

void	SetControlMatrix (const Eigen::Matrix< T, XN, UN > &B)
	Changes the control matrix in the state transition rule.

const Eigen::Matrix< T, XN, XN > &	GetTransitionMatrix () const
	Get the system transition function under zero control.

const Eigen::Matrix< T, XN, XN > &	GetTransitionNoise () const
	Get the covariance matrix of the transition noise.

const Eigen::Matrix< T, ZN, XN > &	GetObservationMatrix () const
	Get the observation matrix, which maps states to observations.

const Eigen::Matrix< T, ZN, ZN > &	GetObservationNoise () const
	Get the covariance matrix of the observation noise.

const Eigen::Matrix< T, XN, UN > &	GetControlMatrix () const
	Get the control matrix in the state transition rule.

void	Predict (const Eigen::Matrix< T, UN, 1 > &u=Eigen::Matrix< T, UN, 1 >::Zero())
	Updates the state belief distribution given the control input u.

void	Correct (const Eigen::Matrix< T, ZN, 1 > &z)
	Updates the state belief distribution given an observation z.

Eigen::Matrix< T, XN, 1 >	GetStateEstimate () const
	Gets mean of our current state belief distribution

Eigen::Matrix< T, XN, XN >	GetStateCovariance () const
	Gets covariance of our current state belief distribution

std::string	DebugString () const
	Gets debug string containing detailed information about the filter.

bool	IsInitialized () const
	Get initialization state of the filter

详细描述

template<typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
class apollo::common::math::KalmanFilter< T, XN, ZN, UN >

Implements a discrete-time Kalman filter.

参数

XN	dimension of state
ZN	dimension of observations
UN	dimension of controls

在文件 kalman_filter.h 第 50 行定义.

构造及析构函数说明

◆ KalmanFilter() [1/2]

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

apollo::common::math::KalmanFilter< T, XN, ZN, UN >::KalmanFilter ( )

inline

Constructor which defers initialization until the initial state distribution parameters are set (with SetStateEstimate), typically on the first observation

在文件 kalman_filter.h 第 57 行定义.

                 {
    F_.setIdentity();
    Q_.setZero();
    H_.setIdentity();
    R_.setZero();
    B_.setZero();
 
    x_.setZero();
    P_.setZero();
    y_.setZero();
    S_.setZero();
    K_.setZero();
  }

◆ KalmanFilter() [2/2]

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

apollo::common::math::KalmanFilter< T, XN, ZN, UN >::KalmanFilter	(	const Eigen::Matrix< T, XN, 1 > &	x,
		const Eigen::Matrix< T, XN, XN > &	P
	)

inline

Constructor which fully initializes the Kalman filter

参数

x	Mean of the state belief distribution
P	Covariance of the state belief distribution

在文件 kalman_filter.h 第 89 行定义.

      : KalmanFilter() {
    SetStateEstimate(x, P);
  }

◆ ~KalmanFilter()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

virtual apollo::common::math::KalmanFilter< T, XN, ZN, UN >::~KalmanFilter ( )

inlinevirtual

Destructor

在文件 kalman_filter.h 第 98 行定义.

98{}

成员函数说明

◆ Correct()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::Correct ( const Eigen::Matrix< T, ZN, 1 > & z )

inline

Updates the state belief distribution given an observation z.

参数

z	Observation

在文件 kalman_filter.h 第 265 行定义.

                                    {
  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_);
}

◆ DebugString()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

std::string apollo::common::math::KalmanFilter< T, XN, ZN, UN >::DebugString ( ) const

inline

Gets debug string containing detailed information about the filter.

返回: Debug string

在文件 kalman_filter.h 第 282 行定义.

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

◆ GetControlMatrix()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

const Eigen::Matrix< T, XN, UN > & apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetControlMatrix ( ) const

inline

Get the control matrix in the state transition rule.

返回: Control matrix

在文件 kalman_filter.h 第 175 行定义.

175{ return B_; }

◆ GetObservationMatrix()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

const Eigen::Matrix< T, ZN, XN > & apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetObservationMatrix ( ) const

inline

Get the observation matrix, which maps states to observations.

返回: Observation matrix

在文件 kalman_filter.h 第 161 行定义.

161{ return H_; }

◆ GetObservationNoise()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

const Eigen::Matrix< T, ZN, ZN > & apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetObservationNoise ( ) const

inline

Get the covariance matrix of the observation noise.

返回: Covariance matrix

在文件 kalman_filter.h 第 168 行定义.

168{ return R_; }

◆ GetStateCovariance()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

Eigen::Matrix< T, XN, XN > apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetStateCovariance ( ) const

inline

Gets covariance of our current state belief distribution

返回: Covariance matrix

在文件 kalman_filter.h 第 204 行定义.

204{ return P_; }

◆ GetStateEstimate()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

Eigen::Matrix< T, XN, 1 > apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetStateEstimate ( ) const

inline

Gets mean of our current state belief distribution

返回: State vector

在文件 kalman_filter.h 第 197 行定义.

197{ return x_; }

◆ GetTransitionMatrix()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

const Eigen::Matrix< T, XN, XN > & apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetTransitionMatrix ( ) const

inline

Get the system transition function under zero control.

返回: Transition matrix.

在文件 kalman_filter.h 第 147 行定义.

147{ return F_; }

◆ GetTransitionNoise()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

const Eigen::Matrix< T, XN, XN > & apollo::common::math::KalmanFilter< T, XN, ZN, UN >::GetTransitionNoise ( ) const

inline

Get the covariance matrix of the transition noise.

返回: Covariance matrix

在文件 kalman_filter.h 第 154 行定义.

154{ return Q_; }

◆ IsInitialized()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

bool apollo::common::math::KalmanFilter< T, XN, ZN, UN >::IsInitialized ( ) const

inline

Get initialization state of the filter

返回: True if the filter is initialized

在文件 kalman_filter.h 第 217 行定义.

217{ return is_initialized_; }

◆ Predict()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::Predict ( const Eigen::Matrix< T, UN, 1 > & u = Eigen::Matrix<T, UN, 1>::Zero() )

inline

Updates the state belief distribution given the control input u.

参数

u	Control input (by default, zero)

在文件 kalman_filter.h 第 255 行定义.

                                    {
  ACHECK(is_initialized_);
 
  x_ = F_ * x_ + B_ * u;
 
  P_ = F_ * P_ * F_.transpose() + Q_;
}

◆ SetControlMatrix()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetControlMatrix ( const Eigen::Matrix< T, XN, UN > & B )

inline

Changes the control matrix in the state transition rule.

参数

B	New control matrix

在文件 kalman_filter.h 第 140 行定义.

140{ B_ = B; }

◆ SetObservationMatrix()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetObservationMatrix ( const Eigen::Matrix< T, ZN, XN > & H )

inline

Changes the observation matrix, which maps states to observations.

参数

H	New observation matrix

在文件 kalman_filter.h 第 119 行定义.

119{ H_ = H; }

◆ SetObservationNoise()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetObservationNoise ( const Eigen::Matrix< T, ZN, ZN > & R )

inline

Changes the covariance matrix of the observation noise.

参数

R	New covariance matrix

在文件 kalman_filter.h 第 126 行定义.

126{ R_ = R; }

◆ SetStateCovariance()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetStateCovariance ( const Eigen::Matrix< T, XN, XN > & P )

inline

Changes the covariance matrix of current state belief distribution.

参数

P	New state covariance matrix

在文件 kalman_filter.h 第 133 行定义.

133{ P_ = P; }

◆ SetStateEstimate()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetStateEstimate	(	const Eigen::Matrix< T, XN, 1 > &	x,
		const Eigen::Matrix< T, XN, XN > &	P
	)

inline

Sets the initial state belief distribution.

参数

x	Mean of the state belief distribution
P	Covariance of the state belief distribution

在文件 kalman_filter.h 第 77 行定义.

                                                         {
    x_ = x;
    P_ = P;
    is_initialized_ = true;
  }

◆ SetTransitionMatrix()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetTransitionMatrix ( const Eigen::Matrix< T, XN, XN > & F )

inline

Changes the system transition function under zero control.

参数

F	New transition matrix

在文件 kalman_filter.h 第 105 行定义.

105{ F_ = F; }

◆ SetTransitionNoise()

template<typename T , unsigned int XN, unsigned int ZN, unsigned int UN>

void apollo::common::math::KalmanFilter< T, XN, ZN, UN >::SetTransitionNoise ( const Eigen::Matrix< T, XN, XN > & Q )

inline

Changes the covariance matrix of the transition noise.

参数

Q	New covariance matrix

在文件 kalman_filter.h 第 112 行定义.

112{ Q_ = Q; }

该类的文档由以下文件生成:

modules/common/math/kalman_filter.h

Public 成员函数

详细描述

构造及析构函数说明

◆ KalmanFilter() [1/2]

◆ KalmanFilter() [2/2]

◆ ~KalmanFilter()

成员函数说明

◆ Correct()

◆ DebugString()

◆ GetControlMatrix()

◆ GetObservationMatrix()

◆ GetObservationNoise()

◆ GetStateCovariance()

◆ GetStateEstimate()

◆ GetTransitionMatrix()

◆ GetTransitionNoise()

◆ IsInitialized()

◆ Predict()

◆ SetControlMatrix()

◆ SetObservationMatrix()

◆ SetObservationNoise()

◆ SetStateCovariance()

◆ SetStateEstimate()

◆ SetTransitionMatrix()

◆ SetTransitionNoise()