/* Follows section 3.1, however we're using the scaled unscented transform. */
void UnscentedKalmanFilter::create_sigma_points() {
Eigen::Matrix<real_t, UKF_STATE_DIM, UKF_STATE_DIM> S;
AssertNormalized(Quaternionr(state.attitude()));
/* Add the process noise before calculating the square root. */
state_covariance.diagonal() += process_noise_covariance;
state_covariance *= UKF_DIM_PLUS_LAMBDA;
AssertPositiveDefinite(state_covariance);
/* Compute the 'square root' of the covariance matrix. */
S = state_covariance.llt().matrixL();
/* Create centre sigma point. */
sigma_points.col(0) = state;
/* For each column in S, create the two complementary sigma points. */
for(uint8_t i = 0; i < UKF_STATE_DIM; i++) {
/*
Construct error quaternions using the MRP method, equation 34 from the
Markley paper.
*/
Vector3r d_p = S.col(i).segment<3>(9);
real_t x_2 = d_p.squaredNorm();
real_t err_w = (-UKF_MRP_A * x_2 +
UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
(UKF_MRP_F_2 + x_2);
Vector3r err_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + err_w)) * d_p;
Quaternionr noise;
noise.vec() = err_xyz;
noise.w() = err_w;
Quaternionr temp;
/* Create positive sigma point. */
sigma_points.col(i+1).segment<9>(0) =
state.segment<9>(0) + S.col(i).segment<9>(0);
temp = noise * Quaternionr(state.attitude());
AssertNormalized(temp);
sigma_points.col(i+1).segment<4>(9) << temp.vec(), temp.w();
sigma_points.col(i+1).segment<12>(13) =
state.segment<12>(13) + S.col(i).segment<12>(12);
/* Create negative sigma point. */
sigma_points.col(i+1+UKF_STATE_DIM).segment<9>(0) =
state.segment<9>(0) - S.col(i).segment<9>(0);
temp = noise.conjugate() * Quaternionr(state.attitude());
AssertNormalized(temp);
sigma_points.col(i+1+UKF_STATE_DIM).segment<4>(9) <<
temp.vec(), temp.w();
sigma_points.col(i+1+UKF_STATE_DIM).segment<12>(13) =
state.segment<12>(13) - S.col(i).segment<12>(12);
}
}
