void Ekf::predict(const double delta)
{
FB_DEBUG("---------------------- Ekf::predict ----------------------\n" <<
"delta is " << delta << "\n" <<
"state is " << state_ << "\n");
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double rollVel = state_(StateMemberVroll);
double pitchVel = state_(StateMemberVpitch);
double yawVel = state_(StateMemberVyaw);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double sp = 0.0;
double cp = 0.0;
::sincos(pitch, &sp, &cp);
double sr = 0.0;
double cr = 0.0;
::sincos(roll, &sr, &cr);
double sy = 0.0;
double cy = 0.0;
::sincos(yaw, &sy, &cy);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = transferFunction_(StateMemberX, StateMemberVx);
transferFunction_(StateMemberRoll, StateMemberVpitch) = transferFunction_(StateMemberX, StateMemberVy);
transferFunction_(StateMemberRoll, StateMemberVyaw) = transferFunction_(StateMemberX, StateMemberVz);
transferFunction_(StateMemberPitch, StateMemberVroll) = transferFunction_(StateMemberY, StateMemberVx);
transferFunction_(StateMemberPitch, StateMemberVpitch) = transferFunction_(StateMemberY, StateMemberVy);
transferFunction_(StateMemberPitch, StateMemberVyaw) = transferFunction_(StateMemberY, StateMemberVz);
transferFunction_(StateMemberYaw, StateMemberVroll) = transferFunction_(StateMemberZ, StateMemberVx);
transferFunction_(StateMemberYaw, StateMemberVpitch) = transferFunction_(StateMemberZ, StateMemberVy);
transferFunction_(StateMemberYaw, StateMemberVyaw) = transferFunction_(StateMemberZ, StateMemberVz);
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double xCoeff = 0.0;
double yCoeff = 0.0;
double zCoeff = 0.0;
double oneHalfATSquared = 0.5 * delta * delta;
yCoeff = cy * sp * cr + sy * sr;
zCoeff = -cy * sp * sr + sy * cr;
double dFx_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dR = 1 + (yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -cy * sp;
yCoeff = cy * cp * sr;
zCoeff = cy * cp * cr;
double dFx_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dP = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -sy * cp;
yCoeff = -sy * sp * sr - cy * cr;
zCoeff = -sy * sp * cr + cy * sr;
double dFx_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFR_dY = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
yCoeff = sy * sp * cr - cy * sr;
zCoeff = -sy * sp * sr - cy * cr;
double dFy_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dR = (yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -sy * sp;
yCoeff = sy * cp * sr;
zCoeff = sy * cp * cr;
double dFy_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dP = 1 + (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = cy * cp;
yCoeff = cy * sp * sr - sy * cr;
zCoeff = cy * sp * cr + sy * sr;
double dFy_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFP_dY = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
yCoeff = cp * cr;
zCoeff = -cp * sr;
double dFz_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFY_dR = (yCoeff * pitchVel + zCoeff * yawVel) * delta;
xCoeff = -cp;
yCoeff = -sp * sr;
zCoeff = -sp * cr;
double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
double dFY_dP = (xCoeff * rollVel + yCoeff * pitchVel + zCoeff * yawVel) * delta;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dFx_dR;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dFx_dP;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dFx_dY;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dFy_dR;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dFy_dP;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dFy_dY;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dFz_dR;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dFz_dP;
transferFunctionJacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR;
transferFunctionJacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP;
transferFunctionJacobian_(StateMemberRoll, StateMemberYaw) = dFR_dY;
transferFunctionJacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR;
transferFunctionJacobian_(StateMemberPitch, StateMemberPitch) = dFP_dP;
transferFunctionJacobian_(StateMemberPitch, StateMemberYaw) = dFP_dY;
transferFunctionJacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;
transferFunctionJacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;
FB_DEBUG("Transfer function is:\n" << transferFunction_ <<
"\nTransfer function Jacobian is:\n" << transferFunctionJacobian_ <<
"\nProcess noise covariance is:\n" << processNoiseCovariance_ <<
"\nCurrent state is:\n" << state_ << "\n");
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
FB_DEBUG("Predicted state is:\n" << state_ <<
"\nCurrent estimate error covariance is:\n" << estimateErrorCovariance_ << "\n");
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ *
estimateErrorCovariance_ *
transferFunctionJacobian_.transpose());
estimateErrorCovariance_.noalias() += (processNoiseCovariance_ * delta);
FB_DEBUG("Predicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
"\n\n--------------------- /Ekf::predict ----------------------\n");
}
void Ekf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double dF0dr = (cy * sp * cr + sy * sr) * delta * yVel + (-cy * sp * sr + sy * cr) * delta * zVel;
double dF0dp = -cy * sp * delta * xVel + cy * cp * sr * delta * yVel + cy * cp * cr * delta * zVel;
double dF0dy = -sy * cp * delta * xVel + (-sy * sp * sr - cy * cr) * delta * yVel + (-sy * sp * cr + cy * sr) * delta * zVel;
double dF1dr = (sy * sp * cr - cy * sr) * delta * yVel + (-sy * sp * sr - cy * cr) * delta * zVel;
double dF1dp = -sy * sp * delta * xVel + sy * cp * sr * delta * yVel + sy * cp * cr * delta * zVel;
double dF1dy = cy * cp * delta * xVel + (cy * sp * sr - sy * cr) * delta * yVel + (cy * sp * cr + sy * sr) * delta * zVel;
double dF2dr = cp * cr * delta * yVel - cp * sr * delta * zVel;
double dF2dp = -cp * delta * xVel - sp * sr * delta * yVel - sp * cr * delta * zVel;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dF0dr;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dF0dp;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dF0dy;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dF1dr;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dF1dp;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dF1dy;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dF2dr;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dF2dp;
if (getDebug())
{
*debugStream_ << "Transfer function is:\n";
*debugStream_ << transferFunction_ << "\n";
*debugStream_ << "Transfer function Jacobian is:\n";
*debugStream_ << transferFunctionJacobian_ << "\n";
*debugStream_ << "Process noise covariance is " << "\n";
*debugStream_ << processNoiseCovariance_ << "\n";
*debugStream_ << "Current state is:\n";
*debugStream_ << state_ << "\n";
}
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Current estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
}
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ * estimateErrorCovariance_ * transferFunctionJacobian_.transpose())
+ (processNoiseCovariance_ * delta);
if (getDebug())
{
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "Last measurement time is:\n";
*debugStream_ << std::setprecision(20) << lastMeasurementTime_ << "\n";
*debugStream_ << "\n--------------------- /Ekf::predict ----------------------\n";
}
}
void Ekf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double xCoeff = 0.0;
double yCoeff = 0.0;
double zCoeff = 0.0;
double oneHalfATSquared = 0.5 * delta * delta;
yCoeff = cy * sp * cr + sy * sr;
zCoeff = -cy * sp * sr + sy * cr;
double dF0dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -cy * sp;
yCoeff = cy * cp * sr;
zCoeff = cy * cp * cr;
double dF0dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -sy * cp;
yCoeff = -sy * sp * sr - cy * cr;
zCoeff = -sy * sp * cr + cy * sr;
double dF0dy = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = sy * sp * cr - cy * sr;
zCoeff = -sy * sp * sr - cy * cr;
double dF1dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -sy * sp;
yCoeff = sy * cp * sr;
zCoeff = sy * cp * cr;
double dF1dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = cy * cp;
yCoeff = cy * sp * sr - sy * cr;
zCoeff = cy * sp * cr + sy * sr;
double dF1dy = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = cp * cr;
zCoeff = -cp * sr;
double dF2dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -cp;
yCoeff = -sp * sr;
zCoeff = -sp * cr;
double dF2dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dF0dr;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dF0dp;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dF0dy;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dF1dr;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dF1dp;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dF1dy;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dF2dr;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dF2dp;
if (getDebug())
{
*debugStream_ << "Transfer function is:\n";
*debugStream_ << transferFunction_ << "\n";
*debugStream_ << "Transfer function Jacobian is:\n";
*debugStream_ << transferFunctionJacobian_ << "\n";
*debugStream_ << "Process noise covariance is " << "\n";
*debugStream_ << processNoiseCovariance_ << "\n";
*debugStream_ << "Current state is:\n";
*debugStream_ << state_ << "\n";
}
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Current estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
}
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ * estimateErrorCovariance_ * transferFunctionJacobian_.transpose())
+ (processNoiseCovariance_ * delta);
if (getDebug())
{
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "\n--------------------- /Ekf::predict ----------------------\n";
}
}
