// C_1_1 = M1 + M4 - M5 + M7 void strassen_calculate_C_1_1( int m, int n, int k, const Dtype* M1, const Dtype* M4, const Dtype* M5, const Dtype* M7, Dtype* C_1_1, int incRowC_1_1 ){ // C_1_1 = M1 + M4 - M5 + M7 // C_1_1 = M1 + M4 matrix_addition(m, k, M1, k, M4, k, C_1_1, incRowC_1_1); // C_1_1 += M7 matrix_addition(m, k, C_1_1, incRowC_1_1, M7, k, C_1_1, incRowC_1_1); // C_1_1 -= M5 matrix_subtraction(m, k, C_1_1, incRowC_1_1, M5, k, C_1_1, incRowC_1_1); }
// C_2_2 = M1 - M2 + M3 + M6 void strassen_calculate_C_2_2( int m, int n, int k, const Dtype* M1, const Dtype* M2, const Dtype* M3, const Dtype* M6, Dtype* C_2_2, int incRowC_2_2 ){ // C_2_2 = M1 - M2 matrix_subtraction(m, k, M1, k, M2, k, C_2_2, incRowC_2_2); // C_2_2 += M3 matrix_addition(m, k, C_2_2, incRowC_2_2, M3, k, C_2_2, incRowC_2_2); // C_2_2 += M6 matrix_addition(m, k, C_2_2, incRowC_2_2, M6, k, C_2_2, incRowC_2_2); }
Dtype* strassen_make_M4_submatrix( const unsigned int m, const unsigned int n, const unsigned int k, const Dtype *A_2_2, const int incRowA_2_2, const Dtype *B_2_1, const int incRowB_2_1, const Dtype *B_1_1, const int incRowB_1_1){ /* construct M4 by the formula M4 = A_2_2 * (B_2_1 - B_1_1) */ Dtype* T1 = make_matrix(m, k); // T1 = B_2_1 - B_1_1 matrix_subtraction(m, k, B_2_1, incRowB_2_1, B_1_1, incRowB_1_1, T1, k); Dtype* M4 = make_matrix(m, k); strassen_mm_worker( m, n, k, A_2_2, incRowA_2_2, T1, k, M4, k); remove_matrix(T1); return M4; }
Dtype* strassen_make_M3_submatrix( const unsigned int m, const unsigned int n, const unsigned int k, const Dtype *A_1_1, const int incRowA_1_1, const Dtype *B_1_2, const int incRowB_1_2, const Dtype *B_2_2, const int incRowB_2_2){ /* construct M3 by the formula M3 = A_1_1 * (B_1_2 - B_2_2) */ // T1 = B_1_2 - B_2_2 Dtype* T1 = make_matrix(m, k); matrix_subtraction(m, k, B_1_2, incRowB_1_2, B_2_2, incRowB_2_2, T1, k); Dtype* M3 = make_matrix(m, k); strassen_mm_worker( m, n, k, A_1_1, incRowA_1_1, T1, k, M3, k); remove_matrix(T1); return M3; }
// M7 = (A_1_2 - A_2_2) * (B_2_1 + B_2_2) Dtype* strassen_make_M7_submatrix( const unsigned int m, const unsigned int n, const unsigned int k, const Dtype *A_1_2, const int incRowA_1_2, const Dtype *A_2_2, const int incRowA_2_2, const Dtype *B_2_1, const int incRowB_2_1, const Dtype *B_2_2, const int incRowB_2_2){ /* construct M7 by the formula M7 = (A_1_2 - A_2_2) * (B_2_1 + B_2_2) */ Dtype* T1 = make_matrix(m, k); Dtype* T2 = make_matrix(m, k); // T1 = (A_1_2 - A_2_2) matrix_subtraction(m, k, A_1_2, incRowA_1_2, A_2_2, incRowA_2_2, T1, k); // T2 = (B_2_1 + B_2_2) matrix_addition(m, k, B_2_1, incRowB_2_1, B_2_2, incRowB_2_2, T2, k); // M7 = T1 * T2 Dtype* M7 = make_matrix(m, k); strassen_mm_worker( m, n, k, T1, k, T2, k, M7, k); remove_matrix(T1); remove_matrix(T2); return M7; }
void kalman(float* in,float* out, float measure_noise, float process_noise) { float R[3][3]={0}, Q[3][3]={0}; /*const float A[3][3]= { {1,0,0}, {0,1,0}, {0,0,1}, }; const float H[3][3]= { {1,0,0}, {0,1,0}, {0,0,1}, }; */ const float B[3][3]= { {1,0,0}, {0,1,0}, {0,0,1}, }; const float I[3][3]= { {1,0,0}, {0,1,0}, {0,0,1}, }; static float x_hat[3][1]; static float P[3][3]; float z[3][1]; float x_hat_[3][1]; float P_[3][3]; float K[3][3]; float P_R1[3][3],P_R2[3][3]; float z_x_hat_[3][1],K_z_x_hat_[3][1]; float I_K[3][3]; R[0][0] = measure_noise*measure_noise; R[1][1] = measure_noise*measure_noise; R[2][2] = measure_noise*measure_noise; Q[0][0] = process_noise*process_noise; Q[1][1] = process_noise*process_noise; Q[2][2] = process_noise*process_noise; matrix_transpose((float*)in,1,3,(float*)z); //-------------------------------------------------------------- // x_hat_ = A*x_hat + B*u_; <=> x_hat_ = B*x_hat matrix_multiply((float*)B, (float*)x_hat,3,3,1,(float*)x_hat_); //-------------------------------------------------------------- //*************************************************************** //P_ = A*P*A' + Q; <=> P_ = P + Q matrix_addition((float*)P,(float*)Q,3,3,(float*)P_); //*************************************************************** //.............................................................. //K = P_*H'*inv(H*P_*H' + R); <=> K = P_*inv(P_ + R) matrix_addition((float*)P_,(float*)R,3,3,(float*)P_R1); // P_R1 = P_ + R matrix_inversion((float*)P_R1,3,(float*)P_R2); // P_R2 = inv( P_R1) matrix_multiply((float*)P_, (float*)P_R2,3,3,3,(float*)K); //.............................................................. //============================================================== //x_hat = x_hat_ + K*(z - H*x_hat_); <=> x_hat = x_hat_ + K*(z - x_hat_) matrix_subtraction((float*)z,(float*)x_hat_,3,1,(float*)z_x_hat_); // z_x_hat_ = z - x_hat_ matrix_multiply((float*)K, (float*)z_x_hat_,3,3,1,(float*)K_z_x_hat_); // K_z_x_hat_ = K*z_x_hat_ matrix_addition((float*)x_hat_,(float*)K_z_x_hat_,3,1,(float*)x_hat); // x_hat = x_hat_ + K_z_x_hat_ //============================================================== //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: //P = ( I - K*H)*P_; <=> ( I - K )*P_ matrix_subtraction((float*)I,(float*)K,3,3,(float*)I_K); // I_K = I - K matrix_multiply((float*)I_K, (float*)P_,3,3,3,(float*)P); // P = I_K*P_ //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: matrix_transpose((float*)x_hat,3,1,(float*)out); }
float kalman_update(float gyroscope_rate, float accelerometer_angle) { static float A[2][2] = {{1.0, -0.019968}, {0.0, 1.0}}; static float B[2][1] = {{0.019968}, {0.0}}; static float C[1][2] = {{1.0, 0.0}}; static float Sz[1][1] = {{17.2}}; static float Sw[2][2] = {{0.005, 0.005}, {0.005, 0.005}}; static float xhat[2][1] = {{0.0}, {0.0}}; static float P[2][2] = {{0.005, 0.005}, {0.005, 0.005}}; float u[1][1]; float y[1][1]; float AP[2][2]; float CT[2][1]; float APCT[2][1]; float CP[1][2]; float CPCT[1][1]; float CPCTSz[1][1]; float CPCTSzInv[1][1]; float K[2][1]; float Cxhat[1][1]; float yCxhat[1][1]; float KyCxhat[2][1]; float Axhat[2][1]; float Bu[2][1]; float AxhatBu[2][1]; float AT[2][2]; float APAT[2][2]; float APATSw[2][2]; float KC[2][2]; float KCP[2][2]; float KCPAT[2][2]; u[0][0] = gyroscope_rate; y[0][0] = accelerometer_angle; matrix_multiply((float*) A, (float*) xhat, 2, 2, 1, (float*) Axhat); matrix_multiply((float*) B, (float*) u, 2, 1, 1, (float*) Bu); matrix_addition((float*) Axhat, (float*) Bu, 2, 1, (float*) AxhatBu); matrix_multiply((float*) C, (float*) xhat, 1, 2, 1, (float*) Cxhat); matrix_subtraction((float*) y, (float*) Cxhat, 1, 1, (float*) yCxhat); matrix_transpose((float*) C, 1, 2, (float*) CT); matrix_multiply((float*) C, (float*) P, 1, 2, 2, (float*) CP); matrix_multiply((float*) CP, (float*) CT, 1, 2, 1, (float*) CPCT); matrix_addition((float*) CPCT, (float*) Sz, 1, 1, (float*) CPCTSz); matrix_multiply((float*) A, (float*) P, 2, 2, 2, (float*) AP); matrix_multiply((float*) AP, (float*) CT, 2, 2, 1, (float*) APCT); matrix_inversion((float*) CPCTSz, 1, (float*) CPCTSzInv); matrix_multiply((float*) APCT, (float*) CPCTSzInv, 2, 1, 1, (float*) K); matrix_multiply((float*) K, (float*) yCxhat, 2, 1, 1, (float*) KyCxhat); matrix_addition((float*) AxhatBu, (float*) KyCxhat, 2, 1, (float*) xhat); matrix_transpose((float*) A, 2, 2, (float*) AT); matrix_multiply((float*) AP, (float*) AT, 2, 2, 2, (float*) APAT); matrix_addition((float*) APAT, (float*) Sw, 2, 2, (float*) APATSw); matrix_multiply((float*) K, (float*) C, 2, 1, 2, (float*) KC); matrix_multiply((float*) KC, (float*) P, 2, 2, 2, (float*) KCP); matrix_multiply((float*) KCP, (float*) AT, 2, 2, 2, (float*) KCPAT); matrix_subtraction((float*) APATSw, (float*) KCPAT, 2, 2, (float*) P); return xhat[0][0]; }
matrix_t matrix_multiplication_strassen(matrix_t mat_a, matrix_t mat_b, matrix_t mat_c, int min_thres) { int len = mat_a.row_end - mat_a.row_start + 1; matrix_t p1, p2, p3, p4, p5, p6, p7; matrix_t c_copy = mat_c; /* if (len != pow(2, log2(len))) { */ /* printf("Strassen Algorithm not applicable\n"); */ /* return mat_c; */ /* } */ if (len <= min_thres) { return matrix_multiplication(mat_a, mat_b, mat_c); } else { printf("Divide and Conquer\n"); matrix_init(&p1, &p2, &p3, &p4, &p5, &p6, &p7, len/2); matrix_multiplication_strassen(partition_matrix(mat_a, 1, 1), matrix_subtraction(partition_matrix(mat_b, 1, 2), partition_matrix(mat_b, 2, 2), p1), p1, min_thres); matrix_multiplication_strassen(partition_matrix(mat_b, 2, 2), matrix_addition(partition_matrix(mat_a, 1, 1), partition_matrix(mat_a, 1, 2), p2), p2, min_thres); matrix_multiplication_strassen(partition_matrix(mat_b, 1, 1), matrix_addition(partition_matrix(mat_a, 2, 1), partition_matrix(mat_a, 2, 2), p3), p3, min_thres); matrix_multiplication_strassen(partition_matrix(mat_a, 2, 2), matrix_subtraction(partition_matrix(mat_b, 2, 1), partition_matrix(mat_b, 1, 1), p3), p3, min_thres); matrix_addition(matrix_multiplication_strassen(partition_matrix(mat_a, 1, 1), matrix_addition(partition_matrix(mat_b, 1, 1), partition_matrix(mat_b, 2, 2), p5), p5, min_thres), matrix_multiplication_strassen(partition_matrix(mat_a, 2, 2), matrix_addition(partition_matrix(mat_b, 1, 1), partition_matrix(mat_b, 2, 2), p5), p5, min_thres), p5); matrix_subtraction(matrix_multiplication_strassen(partition_matrix(mat_a, 1, 2), matrix_addition(partition_matrix(mat_b, 2, 1), partition_matrix(mat_b, 2, 2), p6), p6, min_thres), matrix_multiplication_strassen(partition_matrix(mat_a, 2, 2), matrix_addition(partition_matrix(mat_b, 2, 1), partition_matrix(mat_b, 2, 2), p6), p6, min_thres), p6); matrix_subtraction(matrix_multiplication_strassen(partition_matrix(mat_a, 1, 1), matrix_addition(partition_matrix(mat_b, 1, 1), partition_matrix(mat_b, 1, 2), p7), p7, min_thres), matrix_multiplication_strassen(partition_matrix(mat_a, 2, 1), matrix_addition(partition_matrix(mat_b, 1, 1), partition_matrix(mat_b, 1, 2), p7), p7, min_thres), p7); matrix_addition(matrix_addition(p5, p4, partition_matrix(mat_c, 1, 1)), matrix_subtraction(p6, p2, partition_matrix(mat_c, 1, 1)), partition_matrix(mat_c, 1, 1)); matrix_addition(p1, p2, partition_matrix(mat_c, 1, 2)); matrix_addition(p3, p4, partition_matrix(mat_c, 2, 1)); matrix_subtraction(matrix_addition(p5, p1, partition_matrix(mat_c, 2, 2)), matrix_addition(p3, p7, partition_matrix(mat_c, 2, 2)), partition_matrix(mat_c, 2, 2)); mat_c.row_start = c_copy.row_start; mat_c.row_end = c_copy.row_end; mat_c.column_start = c_copy.column_start; mat_c.column_end = c_copy.column_end; print_matrix(mat_c); return mat_c; } }