int main(){
v4sf a={1,2,3,4}, b={5,6,7,8}, c;
float mat[SIZE][SIZE], vec[SIZE], *res;
int i,j;
for(i=0;i<SIZE;i++) for(j=0;j<SIZE;j++) mat[i][j]=0.+i+j;
for(i=0;i<SIZE;i++) vec[i]=1.;
c = a * b;
printf("%f, %f, %f, %f\n", AR(c)[0], AR(c)[1], AR(c)[2], AR(c)[3] );
printf("size = %d\n",sizeof(v4sf));
res = mv_mult( mat, vec);
for(i=0;i<SIZE;i++) printf("%d %f \n", i, res[i]);
return 0;
}
/*
* Kalman filter update
*
* P is [4,4] and holds the covariance matrix
* R is [3,3] and holds the measurement weights
* C is [4,3] and holds the euler to quat tranform
* X is [4,1] and holds the estimated state as a quaterion
* Xe is [3,1] and holds the euler angles of the estimated state
* Xm is [3,1] and holds the measured euler angles
*/
static inline void
kalman_update(
m_t P,
m_t R,
m_t C,
v_t X,
const v_t Xe,
const v_t Xm
)
{
m_t T1;
m_t T2;
m_t E;
m_t K;
v_t err;
v_t temp;
/* E[3,3] = C[3,4] P[4,4] C'[4,3] + R[3,3] */
m_mult( T1, C, P, 3, 4, 4 );
m_transpose( T2, C, 3, 4 );
m_mult( E, T1, T2, 3, 4, 3 );
m_add( E, R, E, 3, 3 );
/* K[4,3] = P[4,4] C'[4,3] inv(E)[3,3] */
m_mult( T1, P, T2, 4, 4, 3 );
m33_inv( E, T2 );
m_mult( K, T1, T2, 4, 3, 3 );
/* X[4] = X[4] + K[4,3] ( Xm[3] - Xe[3] ) */
v_sub( err, Xm, Xe, 3 );
mv_mult( temp, K, err, 4, 3 );
v_add( X, temp, X, 4 );
/* P[4,4] = P[4,4] - K[4,3] C[3,4] P[4,4] */
m_mult( T1, K, C, 4, 3, 4 );
m_mult( T2, T1, P, 4, 4, 4 );
m_sub( P, T2, P, 4, 4 );
}
/**
* 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 );
}
