/*
* Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
*/
void Ekf::resetWindStates()
{
// get euler yaw angle
matrix::Euler<float> euler321(_state.quat_nominal);
float euler_yaw = euler321(2);
// estimate wind assuming zero sideslip
_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
}
int extGetNextRecord(double *in, int numIn, double *out, int numOut)
{
double inrtl_body[3][3]; /* Inertial to body transformation matrix */
double q_inrtl_body[4]; /* Inertial to body quaternian */
static int status = 0;
static int a[3]; /* Extracted Euler angle sequence */
FILE *pipe;
char i[10];
char j[10];
char k[10];
/* Call Tcl GUI to choose angle sequence */
if (status == 0) {
if ((pipe =
popen("/user3/test_acct/Tcl/choose_sequence.tcl", "r"))
!= NULL) {
fscanf(pipe, "%s %s %s\n", i, j, k);
}
status = 1;
pclose(pipe);
a[0] = atoi(i);
a[1] = atoi(j);
a[2] = atoi(k);
}
/* Set initial matrix with input from SIM. Run matrix functions. */
q_inrtl_body[0] = in[0];
q_inrtl_body[1] = in[1];
q_inrtl_body[2] = in[2];
q_inrtl_body[3] = in[3];
quat_to_mat(inrtl_body, q_inrtl_body);
/* Call external programs according to angle sequence */
if (a[0] == ROLL && a[1] == PITCH && a[2] == YAW)
euler123(out, inrtl_body, 1, out, "", 0);
else if (a[0] == ROLL && a[1] == YAW && a[2] == PITCH)
euler132(out, inrtl_body, 1, out, "", 0);
else if (a[0] == PITCH && a[1] == ROLL && a[2] == YAW)
euler213(out, inrtl_body, 1, out, "", 0);
else if (a[0] == PITCH && a[1] == YAW && a[2] == ROLL)
euler231(out, inrtl_body, 1, out, "", 0);
else if (a[0] == YAW && a[1] == ROLL && a[2] == PITCH)
euler312(out, inrtl_body, 1, out, "", 0);
else if (a[0] == YAW && a[1] == PITCH && a[2] == ROLL)
euler321(out, inrtl_body, 1, out, "", 0);
return (0);
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quaternion quat_before_reset = _state.quat_nominal;
// calculate the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances
Vector3f angle_err_var_vec = calcRotVecVariances();
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate the initial quaternion
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// use a 321 sequence
// rotate the magnetometer measurement into earth frame
matrix::Euler<float> euler321(_state.quat_nominal);
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321);
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler321);
} else {
// use a 312 sequence
// Calculate the 312 sequence euler angles that rotate from earth to body frame
// See http://www.atacolorado.com/eulersequences.doc
Vector3f euler312;
euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
euler312(0) = 0.0f;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
float c2 = cosf(euler312(2));
float s2 = sinf(euler312(2));
float s1 = sinf(euler312(1));
float c1 = cosf(euler312(1));
float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0));
matrix::Dcm<float> R_to_earth;
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// re-calculate the rotation matrix using the updated yaw angle
s0 = sinf(euler312(0));
c0 = cosf(euler312(0));
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(R_to_earth);
}
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// update the yaw angle variance using the variance of the measurement
if (_params.fusion_mode & MASK_USE_EVYAW) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angle_err_var_vec);
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;
return true;
}
