30 #ifndef _RL_MATH_KALMAN_H_
31 #define _RL_MATH_KALMAN_H_
66 template<
typename Vector1,
typename Matrix2,
typename Matrix3,
typename Matrix4,
typename Vector5,
typename Vector6,
typename Matrix7 >
68 const Vector1& xPrior,
69 const Matrix2& pPrior,
78 Matrix k = pPrior * h.transpose() * (h * pPrior * h.transpose() + r).inverse();
80 xPost = xPrior + k * (z - h * xPrior);
82 pPost = (Matrix::Identity(k.rows(), h.cols()) - k * h) * pPrior;
96 template<
typename Vector1,
typename Matrix2,
typename Matrix3,
typename Matrix4,
typename Vector5,
typename Matrix6 >
109 pPrior = a * pPost * a.transpose() + q;
125 template<
typename Vector1,
typename Matrix2,
typename Matrix3,
typename Matrix4,
typename Vector5,
typename Matrix6,
typename Vector7,
typename Matrix8 >
127 const Vector1& xPost,
128 const Matrix2& pPost,
138 xPrior = a * xPost + b * u;
140 pPrior = a * pPost * a.transpose() + q;
151 #endif // _RL_MATH_KALMAN_H_