/* 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); } }