EKF公式推導

離散時間線性高斯系統

運動方程和觀測方差begin{cases} x_k=A_kx_{k-1}+u_k+w_k,& w_k∼N(0,R_k) \ z_k=C_kx_k+v_k, & v_k∼N(0,Q_k) end{cases} tag{16}

其中 k 表示對應時刻下標, u_k,x_k,z_k 是控制、狀態和觀測量,R_k,Q_k 是兩個噪聲項的協方差矩陣。 A_k,C_k 是轉移矩陣和觀測矩陣。


線性系統和KF

根據貝葉斯準則: P(x_k|x_{k-1},u_k,z_k)∝P(z_k|x_k)|P(x_k|x_{k-1},u_k)

所以: N(hat{x_k},hat{P_k})=ηN(C_kx_k,Q) *N(bar x_k,bar P_k)

示例代碼

/**
* @class KalmanFilter
*
* @brief Implements a discrete-time Kalman filter.
*
* @param XN dimension of state
* @param ZN dimension of observations
* @param UN dimension of controls
*/
template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
class KalmanFilter {
public:
......
......
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) {
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) {
y_ = z - H_ * x_;
S_ = H_ * P_ * H_.transpose() + R_;
K_ = P_ * H_.transpose() * PseudoInverse<T, ZN>(S_);
x_ = x_ + K_ * y_;
P_ = (Eigen::Matrix<T, XN, XN>::Identity() - K_ * H_) * P_;
}

赞(0)