void state_init(void)
{
accel_g=sqrt(ahrs_accel[0]*ahrs_accel[0] + ahrs_accel[1]*ahrs_accel[1] + ahrs_accel[2]*ahrs_accel[2]);
accel2euler( raw_ahrs_theta, ahrs_accel, ahrs_compass );
euler2quat( raw_K_Quat, raw_ahrs_theta );
norm( raw_K_Quat );
}
/**
* ahrs_init() takes the initial values from the IMU and converts
* them into the quaterion state vector.
*/
void
ahrs_init(
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
v_t euler_angles;
accel2euler(
euler_angles,
ax,
ay,
az,
heading
);
euler2quat(
X,
euler_angles[0],
euler_angles[1],
euler_angles[2]
);
}
/*--------------------------------------------------------------------------------------*/
void ahrs_init(void)
{
accel2euler( raw_ahrs_theta, imu_accel, ahrs_compass );
euler2quat( raw_K_Quat, raw_ahrs_theta );
norm( raw_K_Quat );
//printf("INIT_q0=%f\tq1=%f\tq2=%f\tq3=%f\n",quat[0],quat[1],quat[2],quat[3] );
//bias[0] = ahrs_pqr[0];
//bias[1] = ahrs_pqr[1];
//bias[2] = ahrs_pqr[2];
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
