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); }