Example #1
0
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);
}
Example #2
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;
}