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