Unscented Kalman Filter
👃

Unscented Kalman Filter

Tags
robotics
Published
October 10, 2020
Author
Chris Chan

Unscented Kalman Filter

The unscented transform can be briefly summarized in 3 steps:
  1. Draw N deterministic samples from an input PDF.
  1. Transform these samples through a nonlinear function.
  1. Use the transformed points to combine the mean and covariance of the output PDF.
This examples uses a similar motion model to the EKF example in the previous post.

State Predict

The state predict is a 3 step procedure
  1. Generate sigma points
  1. Propogate points through motion model
  1. Calculate mean and covariance of sigma points

State Update

The state update is a similar 3 step procedure
  1. Generate sigma points
  1. Propogate points through observation model
  1. Calculate mean and covariance of sigma points

Generating Sigma Points

For a state with L dimensions, we need to generate 2L + 1 sigma points. We assume our state is Gaussian with a mean and covariance
 
N(μx,Σxx)\mathcal{N}(\mu_x , \Sigma_{xx})
 
 
We take the cholesky decomposition of our covariance matrix to obtain a bolded L matrix.
LLT=Σxx\boldsymbol{L}\boldsymbol{L}^T = \Sigma_{xx}
Sigma point xix_i can be represented below
x0=μxx_0 = \mu_x
xi=μx+(L+λ)coliLx_i = \mu_x + \sqrt{(L + \lambda ) col_i \boldsymbol{L}} for i=1,...,Li = 1, ..., L
xi=μx(L+λ)coliLx_{i} = \mu_x - \sqrt{(L + \lambda ) col_i \boldsymbol{L}} for i=L+1,...,2Li = L + 1, ..., 2L
This formula is reflected in the generateSigmaPoints function. We use
γ=L+λ\gamma = \sqrt{L + \lambda}
std::vector<Eigen::Vector4f> generateSigmaPoints( const Eigen::Vector4f& x_est, const Eigen::Matrix4f& P_est, const float gamma) { const size_t L = 4; std::vector<Eigen::Vector4f> sigma_points; sigma_points.reserve(2*L + 1); sigma_points.push_back(x_est); // do cholesky Eigen::Matrix4f P_sqrt ( P_est.llt().matrixL() ); for (size_t i = 0; i < L; ++i) { Eigen::Vector4f pt = x_est + gamma * P_sqrt.col(i); sigma_points.push_back(pt); } for (size_t i = 0; i < L; ++i) { Eigen::Vector4f pt = x_est - gamma * P_sqrt.col(i); sigma_points.push_back(pt); } return sigma_points; }

Recovering Mean and Covariance

After we pass our sigma points through some nonlinear function y=f(xi)y = f(x_i), where ff can be your motion/observation model we need to compute the mean and covariance of the new set of sigma points.
Our weight vector in recovering our mean is
 
wm,i={λλ+Li=012(L+λ)i=1,...,2Lw_{m, i} = \left\{\begin{matrix} \frac{ \lambda }{\lambda+ L} & i = 0\\ \frac{ 1 }{2 (L + \lambda)}& i = 1, ..., 2L \end{matrix}\right.
 
Our weight vector in recovering our covariance is
wc,i={λλ+L+(1α2+β)i=012(L+λ)i=1,...,2Lw_{c, i} = \left\{\begin{matrix} \frac{ \lambda }{\lambda+ L} + ( 1 - \alpha^2 + \beta) & i = 0\\ \frac{ 1 }{2 (L + \lambda)}& i = 1, ..., 2L \end{matrix}\right.
std::tuple<std::vector<float>, std::vector<float>, float> setupWeights(float nx, float alpha, float kappa, float beta) { // nx = L float lamb = std::pow(alpha, 2) * (nx + kappa) - nx; float gamma = std::sqrt(nx + lamb); std::vector<float> wm(2*nx+1, 0.0f); std::vector<float> wc(2*nx+1, 0.0f); wm[0] = lamb / (lamb + nx); wc[0] = ( lamb / (lamb + nx) ) + (1 - std::pow(alpha, 2) + beta); for (size_t i = 1; i < 2*nx+1; ++i) { wm[i] = 1.0f / (2 * (nx + lamb)); wc[i] = 1.0f / (2 * (nx + lamb)); } return {wm, wc, gamma}; }
Using our weights we can calculate the mean and covariance as follows:
μy=wm,iyi\mu_y = \sum w_{m, i} y_i
Σyy=wc,i(yiμy)(yiμy)T\Sigma_{yy} = \sum w_{c, i} (y_i - \mu_y) (y_i - \mu_y)^T
 
The summation of the weights with the individual sigma points for the motion model and observation model are very similar. We use a gain matrix K (similar to EKF) to propogate our state estimate.
void unscentedKalmanFilter(Eigen::Vector4f& x_est, Eigen::Matrix4f& P_est, const Eigen::Vector2f& u, const Eigen::Vector2f& z, const Eigen::Matrix4f& Q, const Eigen::Matrix2f& R, const float dt, const float gamma, const std::vector<float>& wm, const std::vector<float>& wc) { std::vector<Eigen::Vector4f> sigma_points; // STATE PREDICT // generate and propogate sigma points through motion model sigma_points = generateSigmaPoints(x_est, P_est, gamma); for (auto& pt : sigma_points) pt = motionModel(pt, u, dt); // recombine sigma points to get predict mean and cov Eigen::Vector4f x_pred = Eigen::Vector4f::Zero(4); for (size_t i = 0; i < sigma_points.size(); ++i) x_pred += wm[i] * sigma_points[i]; Eigen::Matrix4f P_pred = Q; for (size_t i = 0; i < sigma_points.size(); ++i) { Eigen::Vector4f diff = (sigma_points[i] - x_pred); P_pred += wc[i] * diff * diff.transpose(); } // STATE CORRECT Eigen::Vector2f z_pred = observationModel(x_pred); Eigen::Vector2f y = z - z_pred; // generate sigma points, transform to observation domain sigma_points = generateSigmaPoints(x_pred, P_pred, gamma); std::vector<Eigen::Vector2f> z_sigma_points; for (auto& pt : sigma_points) z_sigma_points.emplace_back( observationModel(pt) ); Eigen::Vector2f z_mean = Eigen::Vector2f::Zero(2); for (size_t i = 0; i < sigma_points.size(); ++i) z_mean += wm[i] * z_sigma_points[i]; Eigen::Matrix2f cov_zz = R; // st for (size_t i = 0; i < z_sigma_points.size(); ++i) { Eigen::Vector2f diff = (z_sigma_points[i] - z_mean); cov_zz += wc[i] * diff * diff.transpose(); } // Eigen::Matrix<float, 4, 2> cov_xz; Eigen::Matrix<float, 4, 2> cov_xz = Eigen::MatrixXf::Zero(4,2); for (size_t i = 0; i < z_sigma_points.size(); ++i) { Eigen::Vector2f diffz = (z_sigma_points[i] - z_mean); Eigen::Vector4f diffx = (sigma_points[i] - x_pred); cov_xz += wc[i] * diffx * diffz.transpose(); } auto K = cov_xz * cov_zz.inverse(); // (4x2) * (2,2) => (4,2) x_est = x_pred + K * y; // (4,1) + (4,2) * (2,1) P_est = P_pred - K * cov_zz * K.transpose(); // (4,4) - (4,2)*(2,2)*(2,4) }

Sources