attitude_estimation.c File Reference

#include "attitude_estimation.h"
#include "eye.h"
#include <math.h>

Include dependency graph for attitude_estimation.c:

Go to the source code of this file.

Functions
void	attitude_estimation (const double quat[4], double ts, double omega_x, double omega_y, double omega_z, double *alpha, double quat_new[4])
	Propagates the rocket's attitude quaternion and calculates the angle from vertical.

Function Documentation

attitude_estimation()

void attitude_estimation	(	const double	quat[4],
		double	ts,
		double	omega_x,
		double	omega_y,
		double	omega_z,
		double *	alpha,
		double	quat_new[4]
	)

Propagates the rocket's attitude quaternion and calculates the angle from vertical.

Parameters

[in]	quat	The current attitude quaternion [w, x, y, z].
[in]	ts	The time step (delta time) in seconds since the last update.
[in]	omega_x	Angular velocity around the body's X-axis in radians/second.
[in]	omega_y	Angular velocity around the body's Y-axis in radians/second.
[in]	omega_z	Angular velocity around the body's Z-axis in radians/second.
[out]	alpha	Pointer to a double to store the calculated angle between the rocket's body axis and the vertical, in degrees.
[out]	quat_new	An array to store the resulting, updated attitude quaternion.

Returns

None

Definition at line 27 of file attitude_estimation.c.

{
  double dv[16];
  double a;
  double a_idx_0_tmp;
  double a_idx_1_tmp;
  double a_idx_2_tmp;
  double a_idx_3_tmp;
  double b_a_idx_0_tmp;
  double b_a_idx_2_tmp;
  double c_a_idx_0_tmp;
  int i;
  a = 0.5 * ts;
  eye_attitude(dv);
  a_idx_0_tmp = a * 0.0;
  a_idx_1_tmp = a * -omega_x;
  a_idx_2_tmp = a * -omega_y;
  a_idx_3_tmp = a * -omega_z;
  b_a_idx_0_tmp = a * omega_x;
  b_a_idx_2_tmp = a * omega_z;
  c_a_idx_0_tmp = a * omega_y;
  dv[0] += a_idx_0_tmp;
  dv[1] += b_a_idx_0_tmp;
  dv[2] += c_a_idx_0_tmp;
  dv[3] += b_a_idx_2_tmp;
  dv[4] += a_idx_1_tmp;
  dv[5] += a_idx_0_tmp;
  dv[6] += a_idx_3_tmp;
  dv[7] += c_a_idx_0_tmp;
  dv[8] += a_idx_2_tmp;
  dv[9] += b_a_idx_2_tmp;
  dv[10] += a_idx_0_tmp;
  dv[11] += a_idx_1_tmp;
  dv[12] += a_idx_3_tmp;
  dv[13] += a_idx_2_tmp;
  dv[14] += b_a_idx_0_tmp;
  dv[15] += a_idx_0_tmp;
  a = quat[0];
  c_a_idx_0_tmp = quat[1];
  a_idx_0_tmp = quat[2];
  a_idx_1_tmp = quat[3];
  for (i = 0; i < 4; i++) {
    quat_new[i] =
        ((dv[i] * a + dv[i + 4] * c_a_idx_0_tmp) + dv[i + 8] * a_idx_0_tmp) +
        dv[i + 12] * a_idx_1_tmp;
  }
  *alpha = acos(2.0 * (quat[0] * quat[0] + quat[3] * quat[3]) - 1.0);
}

References alpha, and eye_attitude().

Referenced by StartKalman_mkf().

Here is the call graph for this function:

Here is the caller graph for this function: