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
}
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;
}
void graphMatching(int size1, float G1[size1][size1], int size2, float G2[size2][size2], float sigma, int numberOfMatches, float X[size1][size2], float Z[size1][size2], float Y[size1][size2]){
/* Algorithm due to R. Zass and A. Shashua.,
'Probabilistic Graph and Hypergraph Matching.',
Computer Vision and Pattern Recognition (CVPR) Anchorage, Alaska, June 2008.
G1 An size1 by size1 symmetric matrix, with the weight of the first graph edges.
G2 An size2 by size2 symmetric matrix, with the weight of the second graph edges.
sigma Kernel parameter for edge-to-edge correlations.
numberOfMatches number of matches required.
X [Output] a size1 by size2 matrix with the hard matching results.
The i,j entry is one iff the i-th feature of the first object
match the j-th feature of the second object. Zero otherwise.
Z [Output] a size1 by size2 matrix with the soft matching results.
The i,j entry is the probablity that the i-th feature of the
first object match the j-th feature of the second object.
Y [Output] Debug information.
*/
//Check to make sure the matrices are symmetric
if(isSymmetric(size1, G1) == 0) {
printf("G1 is not symmetric \n");
}
if(isSymmetric(size2, G2) == 0) {
printf("G2 is not symmetric \n");
}
// tranpose G2
float G2t[size2][size2];
transpose(size2, size2, G2, G2t);
// make Y an all zero matrix
zeros(size1, size2, Y);
float d[size1][size2], d1[size1][size2], d2[size1][size2], G1_i[size1][1], G2t_j[1][size2];
for(int i=0; i < size1; i++) {
for(int j=0; j < size2; j++) {
//d = repmat(G1(:,i), 1, n2) - repmat(G2t(j, :), n1, 1);
//...G1(:,i)...
getCol(size1, size1, G1, G1_i, i);
// ...G2t(j,:)...
getRow(size2, size2, G2t, G2t_j, j);
//...repmat(G1(:,i), 1, n2)...
repmat(size1, 1, G1_i, 1, size2, d1);
//...repmat(G2t(j, :), n1, 1);
repmat(1, size2, G2t_j, size1, 1, d2);
// d = ... - ...
matSub(size1, size2, d1, d2, d);
//Y = Y + exp((-d.*d) ./ sigma);
//exp((-d.*d) ./ sigma)
for(int k=0; k < size1; k++) {
for(int l=0; l < size2; l++) {
d[k][l] = exp((-d[k][l] * d[k][l]) / sigma);
}
}
// Y = Y + ...
for(int u=0; u < size1; u++) {
for(int v=0; v < size2; v++) {
Y[u][v] += d[u][v];
}
}
}
}
/* do hypergraphMatching over Y */
hypergraphMatching(size1, size2, Y, numberOfMatches, X, Z);
}
