void generate_system_transfer( uFloat *state, uFloat **phi )
{
int row, col;
#ifdef PRINT_DEBUG
printf( "ekf: linearizing system transfer\n" );
#endif
for( row = 1; row <= STATE_SIZE; row++ )
for( col = 1; col <= STATE_SIZE; col++ )
phi[ row ][ col ] = 0.0;
for( col = 1; col <= STATE_SIZE; col++ )
phi[ col ][ col ] = 1.0;
systemJacobian( state, temp_state_state );
matMultScalar( temp_state_state, PERIOD, temp_state_state,
STATE_SIZE, STATE_SIZE );
matAdd( phi, temp_state_state, phi, STATE_SIZE, STATE_SIZE );
}
static void update_prob( uFloat **P_pre, uFloat **R, uFloat **H,
uFloat **P_post, uFloat **K )
{
#ifdef PRINT_DEBUG
printf( "ekf: updating prob\n" );
#endif
#ifdef DIV_DEBUG
printMatrix( "P", P_pre, STATE_SIZE, STATE_SIZE );
#endif
#ifdef DIV_DEBUG
printMatrix( "H", H, MEAS_SIZE, STATE_SIZE );
#endif
#ifdef DIV_DEBUG
printMatrix( "R", R, MEAS_SIZE, MEAS_SIZE );
#endif
matMultTranspose( P_pre, H, temp_state_meas,
STATE_SIZE, STATE_SIZE, MEAS_SIZE ); /* temp_state_meas = P_pre*H' */
matMult( H, temp_state_meas, temp_meas_meas,
MEAS_SIZE, STATE_SIZE, MEAS_SIZE ); /* temp_meas_meas = (H*P_pre)*H' */
matAdd( temp_meas_meas, R, temp_meas_meas,
MEAS_SIZE, MEAS_SIZE );
take_inverse( temp_meas_meas, temp_meas_2, MEAS_SIZE ); /* temp_meas_2 = inv( H*P_pre*H' + R) */
#ifdef DIV_DEBUG
printMatrix( "1 / (HPH + R)", temp_meas_2,
MEAS_SIZE, MEAS_SIZE );
#endif
matMult( temp_state_meas, temp_meas_2, K,
STATE_SIZE, MEAS_SIZE, MEAS_SIZE ); /* K = P_pre * H' * (inv( H*P_pre*H' + R))' */
#ifdef DIV_DEBUG
printMatrix( "Kalman Gain", K, STATE_SIZE, MEAS_SIZE );
#endif
matMult( H, P_pre, temp_meas_state,
MEAS_SIZE, STATE_SIZE, STATE_SIZE ); /* temp_meas_state = H * P_pre */
matMult( K, temp_meas_state, temp_state_state,
STATE_SIZE, MEAS_SIZE, STATE_SIZE ); /* temp_state_state = K*H*P_pre */
#ifdef PRINT_DEBUG
printf( "ekf: updating prob 3\n" );
#endif
matSub( P_pre, temp_state_state, P_post, STATE_SIZE, STATE_SIZE ); /* P_post = P_pre - K*H*P_pre */
#ifdef DIV_DEBUG
printMatrix( "New P_post", P_post,
STATE_SIZE, STATE_SIZE );
#endif
}
static void estimate_prob( uFloat **P_post, uFloat **Phi, uFloat **Qk,
uFloat **P_pre )
{
#ifdef PRINT_DEBUG
printf( "ekf: estimating prob\n" );
#endif
matMultTranspose( P_post, Phi, temp_state_state,
STATE_SIZE, STATE_SIZE, STATE_SIZE ); /* temp_state_state = P_post * Phi' */
matMult( Phi, temp_state_state, P_pre,
STATE_SIZE, STATE_SIZE, STATE_SIZE ); /* P_pre = Phi * P_post * Phi' */
matAdd( P_pre, Qk, P_pre, STATE_SIZE, STATE_SIZE ); /* P_pre = Qk + Phi * P_post * Phi' */
}
/*
* Returns the *transposed* Jacobian matrix of the matrix cross product
* r12 x r23 = (r2 - r1) x (r3 - r2)
* where r1, r2 and r3 are vectors and differentiation is done with regards
* to ri (with i the given parameter) with all other rj (j != i) assumed to
* be fixed and independent of ri.
*
* The math is worked out in:
* http://www-personal.umich.edu/~riboch/pubfiles/riboch-JacobianofCrossProduct.pdf
* The result:
* J_ri(r12 x r23) = r12^x * J_ri(r23) - r23^x * J_ri(r12)
* where r12^x and r23^x are the matrix form of the cross product:
* ( 0 -Az Ay)
* A^x = ( Az 0 -Ax)
* (-Ay Ax 0 )
* and J_ri(r12) and J_ri(r23) are the jacobi matrices for r12 and r23,
* which are either 0, or +/- the identity matrix.
* We have:
* J_r1(r12) = -1, J_r2(r12) = +1, J_r3(r12) = 0,
* J_r1(r23) = 0, J_r2(r23) = -1, J_r3(r23) = +1.
*/
static __inline__ Mat3 crossProdTransposedJacobian(Vec3 r12, Vec3 r23, int i)
{
/* *Transposed* matrix form of the cross product. */
Mat3 r12matrCrossProd = mat3( 0, r12.z, -r12.y,
-r12.z, 0, r12.x,
r12.y, -r12.x, 0 );
/* *Transposed* matrix form of the cross product. */
Mat3 r23matrCrossProd = mat3( 0, r23.z, -r23.y,
-r23.z, 0, r23.x,
r23.y, -r23.x, 0 );
switch (i) {
case 1:
return r23matrCrossProd;
case 2:
return matScale(matAdd(r12matrCrossProd, r23matrCrossProd), -1);
case 3:
return r12matrCrossProd;
default:
die("Internal error!\n");
return mat3(0,0,0,0,0,0,0,0,0); /* To make compiler happy. */
}
}
void kfUpdate(float *xl, float *omega) {
static float phi = 0;
static float theta = 0;
static float psi = 0;
float dphi, dtheta, dP;
float sin_theta;
float cos_phi = cos(phi);
float sin_phi = sin(phi);
float cos_theta = cos(theta);
float tan_theta = tan(theta);
float a_values[] = { -wy*cos_phi*tan_theta + wz*sin_phi*tan_theta,
(-wy*sin_phi + wz*cos_phi) / (cos_theta*cos_theta),
wy*sin_phi - wz*cos_phi,
0 };
float c_values[6];
float h_values[3];
matAssignValues(A, a_values);
dphi = wx - wy*sin_phi*tan_theta - wz*cos_phi*tan_theta;
dtheta = -wy*cos_phi - wz*sin_phi;
phi = phi + dphi * TIME_STEP;
theta = theta + dtheta * TIME_STEP;
// computing dP = APA' + Q
matTranspose(temp2x2, A); // A'
matDotProduct(temp2x2, P, temp2x2); // PA'
matDotProduct(temp2x2, A, temp2x2); // APA'
matAdd(dP, temp2x2, Q); // APA' + Q
// computing P = P + dt*dP
matScale(temp2x2, dP, TIME_STEP); // dt*dP
matAdd(P, P, temp2x2); // P + dt*dP
cos_phi = cos(phi);
sin_phi = sin(phi);
cos_theta = cos(theta);
sin_theta = sin(theta);
c_values = {0, cos_theta,
-cos_theta*cos_phi, sin_theta*sin_phi,
cos_theta*sin_phi, sin_theta*cos_phi }
matAssignValues(C, c_values);
// L = PC'(R + CPC')^-1
matTranspose(temp2x3, C); // C'
matDotProduct(temp2x3, P, temp2x3); // PC'
matDotProduct(temp3x3, C, temp2x3); // CPC'
matAdd(temp3x3, R, temp3x3); // R + CPC'
matInverse(temp3x3, temp3x3); // (R + CPC')^-1
matDotProduct(L, temp2x3, temp3x3); // PC'(R + CPC')^-1
// P = (I - LC)P
matDotProduct(temp2x2, L, C); // LC
matSub(temp2x2, I, temp2x2); // I - LC
matDotProduct(P, temp2x2, P); // (I - LC)P
h_values = {sin_theta, -cos_theta*sin_phi, -cos_theta*cos_phi};
matAssignValues(H, h_values);
/*
ph = ph + dot(L[0], self.ab - h)
th = th + dot(L[1], self.ab - h)
*/
// change the values so that they stay between -PI and +PI
phi = ((phi + PI) % (2*PI)) - PI;
theta = ((theta + PI) % (2*PI)) - PI;
psidot = wy * sin(ph) / cos(th) + * cos(ph) / cos(th);
psi += psidot * TIME_STEP;
}
