// http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf
Quaternionr Leapfrog::DotQ(const Vector3r& angVel, const Quaternionr& Q){
Quaternionr dotQ;
dotQ.w() = (-Q.x()*angVel[0]-Q.y()*angVel[1]-Q.z()*angVel[2])/2;
dotQ.x() = ( Q.w()*angVel[0]-Q.z()*angVel[1]+Q.y()*angVel[2])/2;
dotQ.y() = ( Q.z()*angVel[0]+Q.w()*angVel[1]-Q.x()*angVel[2])/2;
dotQ.z() = (-Q.y()*angVel[0]+Q.x()*angVel[1]+Q.w()*angVel[2])/2;
return dotQ;
}
/* 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);
}
}
/*
Solve the initial value problems in order to set up continuity constraints,
which effectively store the system dynamics for this SQP iteration.
At the same time, compute the Jacobian function by applying perturbations to
each of the variables in turn, for use in the continuity constraints.
*/
void OptimalControlProblem::solve_ivps(uint32_t i) {
uint32_t j;
/* Solve the initial value problem at this horizon step. */
integrated_state_horizon[i] = integrator.integrate(
State(state_reference[i]),
control_reference[i],
dynamics,
OCP_STEP_LENGTH);
for(j = 0; j < NMPC_GRADIENT_DIM; j++) {
ReferenceVector perturbed_state;
perturbed_state.segment<NMPC_STATE_DIM>(0) = state_reference[i];
perturbed_state.segment<NMPC_CONTROL_DIM>(NMPC_STATE_DIM) =
control_reference[i];
StateVector new_state;
real_t perturbation = NMPC_EPS_4RT;
/* Need to calculate quaternion perturbations using MRPs. */
if(j < 6) {
perturbed_state[j] += perturbation;
} else if(j >= 6 && j <= 8) {
Vector3r d_p;
d_p << 0.0, 0.0, 0.0;
d_p[j-6] = perturbation;
real_t x_2 = d_p.squaredNorm();
real_t delta_w = (-NMPC_MRP_A * x_2 + NMPC_MRP_F * std::sqrt(
NMPC_MRP_F_2 + ((real_t)1.0 - NMPC_MRP_A_2) * x_2)) /
(NMPC_MRP_F_2 + x_2);
Vector3r delta_xyz = (((real_t)1.0 / NMPC_MRP_F) *
(NMPC_MRP_A + delta_w)) * d_p;
Quaternionr delta_q;
delta_q.vec() = delta_xyz;
delta_q.w() = delta_w;
Quaternionr temp = delta_q *
Quaternionr(perturbed_state.segment<4>(6));
perturbed_state.segment<4>(6) << temp.vec(), temp.w();
} else if(j < NMPC_DELTA_DIM) {
perturbed_state[j+1] += perturbation;
} else {
/*
Perturbations for the control inputs should be proportional
to the control range to make sure we don't lose too much
precision.
*/
perturbation *=
(upper_control_bound[j-NMPC_DELTA_DIM] -
lower_control_bound[j-NMPC_DELTA_DIM]);
perturbed_state[j+1] += perturbation;
}
new_state.segment<NMPC_STATE_DIM>(0) = integrator.integrate(
State(perturbed_state.segment<NMPC_STATE_DIM>(0)),
perturbed_state.segment<NMPC_CONTROL_DIM>(NMPC_STATE_DIM),
dynamics,
OCP_STEP_LENGTH);
/*
Calculate delta between perturbed state and original state, to
yield a full column of the Jacobian matrix.
*/
jacobians[i].col(j) =
state_to_delta(integrated_state_horizon[i], new_state) /
perturbation;
}
/*
Calculate integration residuals; these are needed for the continuity
constraints.
*/
integration_residuals[i] = state_to_delta(
state_reference[i+1],
integrated_state_horizon[i]);
}