// 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); }
// model of motor subsystem in physics-based model of wheelchair(Quantum6000z) void motor_model_c(double *motor_state, double *friction_out, double motor_input, double delta_t, double param_mu, double param_beta, double param_gamma, double param_alpha){ double friction_threshold; mxArray *mtx_A, *mtx_Bf, *mtx_Bd, *mtx_tmp1, *mtx_tmp2, *mtx_tmp3; double *mtx_A_ptr, *mtx_Bf_ptr, *mtx_Bd_ptr, *mtx_tmp1_ptr, *mtx_tmp2_ptr, *mtx_tmp3_ptr; // parameter matrix set-up mtx_A = mxCreateDoubleMatrix(2, 2, mxREAL); mtx_A_ptr = mxGetPr(mtx_A); mtx_A_ptr[0] = 1; mtx_A_ptr[1] = -param_beta * delta_t; mtx_A_ptr[2] = delta_t; mtx_A_ptr[3] = 1 - param_gamma * delta_t; mtx_Bf = mxCreateDoubleMatrix(2, 1, mxREAL); mtx_Bf_ptr = mxGetPr(mtx_Bf); mtx_Bf_ptr[0] = delta_t; mtx_Bf_ptr[1] = 0; mtx_Bd = mxCreateDoubleMatrix(2, 1, mxREAL); mtx_Bd_ptr = mxGetPr(mtx_Bd); mtx_Bd_ptr[0] = 0; mtx_Bd_ptr[1] = param_alpha * delta_t; // temporary variable set-up mtx_tmp1 = mxCreateDoubleMatrix(2, 1, mxREAL); mtx_tmp1_ptr = mxGetPr(mtx_tmp1); mtx_tmp2 = mxCreateDoubleMatrix(2, 1, mxREAL); mtx_tmp2_ptr = mxGetPr(mtx_tmp2); mtx_tmp3 = mxCreateDoubleMatrix(2, 1, mxREAL); mtx_tmp3_ptr = mxGetPr(mtx_tmp3); // friction threshold set-up friction_threshold = -motor_state[0]/delta_t - motor_state[1]; if(abs(friction_threshold) <= param_mu) friction_out[0] = friction_threshold; else friction_out[0] = sign_manual(friction_threshold) * param_mu; // update motor state(motor_state = A*motor_state + B_f*friction_out + B_d*motor_input) matrix_multiplication(mtx_A_ptr, motor_state, mtx_tmp1_ptr, 2, 1, 2); //matrix_scalar_multiplication(mtx_Bf_ptr, friction_out, mtx_tmp2_ptr, 2, 1); matrix_multiplication(mtx_Bf_ptr, friction_out, mtx_tmp2_ptr, 2, 1, 1); matrix_addition(mtx_tmp1_ptr, mtx_tmp2_ptr, mtx_tmp3_ptr, 2, 1); matrix_scalar_multiplication(mtx_Bd_ptr, motor_input, mtx_tmp2_ptr, 2, 1); matrix_addition(mtx_tmp2_ptr, mtx_tmp3_ptr, mtx_tmp1_ptr, 2, 1); matrix_assignment(mtx_tmp1_ptr, motor_state, 2, 1); return; }
Dtype* strassen_make_M2_submatrix( const unsigned int m, const unsigned int n, const unsigned int k, const Dtype *A_2_1, const int incRowA_2_1, const Dtype *A_2_2, const int incRowA_2_2, const Dtype *B_1_1, const int incRowB_1_1){ /* construct M2 by the formula M2 = (A_2_1 + A_2_2) * B_1_1 */ // T1 = A_2_1 + A_2_2 Dtype* T1 = make_matrix(m, k); matrix_addition(m, k, A_2_1, incRowA_2_1, A_2_2, incRowA_2_2, T1, k); Dtype* M2 = make_matrix(m, k); strassen_mm_worker( m, n, k, T1, k, B_1_1, incRowB_1_1, M2, k); remove_matrix(T1); return M2; }
Dtype* strassen_make_M5_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 *A_1_2, const int incRowA_1_2, const Dtype *B_2_2, const int incRowB_2_2){ /* construct M5 by the formula M5 = (A_1_1 + A_1_2) * B_2_2 */ // T1 = A_1_1 + A_1_2 Dtype* T1 = make_matrix(m, k); matrix_addition(m, k, A_1_1, incRowA_1_1, A_1_2, incRowA_1_2, T1, k); Dtype* M5 = make_matrix(m, k); strassen_mm_worker( m, n, k, T1, k, B_2_2, incRowB_2_2, M5, k); remove_matrix(T1); return M5; }
void strassen_calculate_C_2_1( int m, int n, int k, const Dtype* M2, const Dtype* M4, Dtype* C_2_1, int incRowC_2_1 ){ matrix_addition(m, k, M2, k, M4, k, C_2_1, incRowC_2_1); }
void strassen_calculate_C_1_2( int m, int n, int k, const Dtype* M3, const Dtype* M5, Dtype* C_1_2, int incRowC_1_2 ){ matrix_addition(m, k, M3, k, M5, k, C_1_2, incRowC_1_2); }
// M1 = (A_1_1 + A_2_2) * (B_1_1 + B_2_2) Dtype* strassen_make_M1_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 *A_2_2, const int incRowA_2_2, const Dtype *B_1_1, const int incRowB_1_1, const Dtype *B_2_2, const int incRowB_2_2){ /* construct M1 by the formula M1 = (A_1_1 + A_2_2) * (B_1_1 + B_2_2) */ // T1 = (A_1_1 + A_2_2) Dtype* T1 = make_matrix(m, k); matrix_addition(m, k, A_1_1, incRowA_1_1, A_2_2, incRowA_2_2, T1, k); // T2 = (B_1_1 + B_2_2) Dtype* T2 = make_matrix(m, k); matrix_addition(m, k, B_1_1, incRowB_1_1, B_2_2, incRowB_2_2, T2, k); // M1 = T1 * T2 Dtype* M1 = make_matrix(m, k); strassen_mm_worker( m, n, k, T1, k, T2, k, M1, k); remove_matrix(T1); remove_matrix(T2); return M1; }
// 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); }
/* * Name : PPCFFRELS_Update * Description : 渐消记忆递推最小二乘辨识,这里是闭环的系统辨识,输入的都是经过滤波以后的序列 * Entry : PPCFFRELS_T的结构体指针,经滤波后的输出时间序列(当先为0越大越过去),经滤波后的输入时间序列(当先为0越大越过去),遗忘因子(0.9-1) * Return : void * Author : lynx [email protected]. * * History * ---------------------- * Rev : 0.00 * Date : 06/14/2013 * * create. * ---------------------- * Rev : 0.00 * Date : 06/17/2013 * * 将FGR的运算独立了 * ---------------------- */ void PPCFFRELS_Update(PPCFFRELS_T* relsIn, float CDataYFK[], float CDataUFK[], float lambda) { //这里先都按照最长情况分配地址,计算时只计算限制的范围 float RELS_tmp_V1[PPCFFRELS_ML_A][1]; //用于矩阵运算的临时变量 向量 float RELS_tmp_VT1[1][PPCFFRELS_ML_A]; //用于矩阵运算的临时变量 转置向量 float RELS_tmp_VT2[1][PPCFFRELS_ML_A]; //用于矩阵运算的临时变量 转置向量 float RELS_tmp_M1[PPCFFRELS_ML_A][PPCFFRELS_ML_A]; //用于矩阵运算的临时变量 矩阵 float RELS_tmp_M2[PPCFFRELS_ML_A][PPCFFRELS_ML_A]; //用于矩阵运算的临时变量 矩阵 float RELS_tmp_M3[PPCFFRELS_ML_A][PPCFFRELS_ML_A]; //用于矩阵运算的临时变量 矩阵 float RELS_tmp_M4[PPCFFRELS_ML_A][PPCFFRELS_ML_A]; //用于矩阵运算的临时变量 矩阵 float RELS_tmp_UM1[1][1]; //用于矩阵运算的临时变量 单位矩阵 float RELS_tmp_U1 = 0; //用于矩阵运算的临时变量 中间变量 int RELS_i = 0; //循环用的变量 //规范化数据 if(lambda>1){ lambda = 1; }else if(lambda<0.9){ lambda = 0.9; } //---------------------------------------------------------------------------------------- //thetae_1=thetae(:,k); 这句被从最后移到了这里,为了保证代码的集中,方便移植 matrix_copy((float*)relsIn->thetae, relsIn->ML, 1, (float*)relsIn->thetae_1); //首先构造观测向量ψ phie=[ufk(d:d+nf1);yfk(d:d+ng)]; %这里观测的是除掉△的F //注意这里没有后推,因为有D的存在干扰了 for(RELS_i=0;RELS_i<(relsIn->NF+1);RELS_i++){ //ufk(d:d+nf1) relsIn->phie[RELS_i][0] = CDataUFK[relsIn->D+RELS_i]; //这里要注意有D的项不要额外+1 D=1时从过去的第一个也就是现在的第二个也就是[1]开始 } for(RELS_i=0;RELS_i<(relsIn->NG+1);RELS_i++){ //yfk(d:d+ng) relsIn->phie[relsIn->NF+1+RELS_i][0] = CDataYFK[relsIn->D+RELS_i]; //这里要注意有D的项不要额外+1 } //K=P*phie/(lambda+phie'*P*phie); matrix_transpose((float*)relsIn->phie, relsIn->ML, 1, (float*)RELS_tmp_VT1); //phie' matrix_multiply((float*)RELS_tmp_VT1, (float*)relsIn->P, 1, relsIn->ML, relsIn->ML, (float*)RELS_tmp_VT2); //phie'*P matrix_multiply((float*)RELS_tmp_VT2, (float*)relsIn->phie, 1, relsIn->ML, 1, (float*)RELS_tmp_UM1); //phie'*P*phie RELS_tmp_U1 = 1.0/(lambda+RELS_tmp_UM1[0][0]); ///(lambda+phie'*P*phie) matrix_multiply((float*)relsIn->P, (float*)relsIn->phie, relsIn->ML, relsIn->ML, 1, (float*)RELS_tmp_V1); //P*phie matrix_multiply_k((float*)RELS_tmp_V1, RELS_tmp_U1, relsIn->ML, 1, (float*)relsIn->K); //K=P*phie/(lambda+phie'*P*phie); //thetae(:,k)=thetae_1+K*(y(k)-phie'*thetae_1); matrix_multiply((float*)RELS_tmp_VT1, (float*)relsIn->thetae_1, 1, relsIn->ML, 1, (float*)RELS_tmp_UM1); //phie'*thetae_1 前面已经算过phie'了,这里直接用,前面注意保留 RELS_tmp_U1 = CDataYFK[0]-RELS_tmp_UM1[0][0]; //y(k)-phie'*thetae_1 matrix_multiply_k((float*)relsIn->K, RELS_tmp_U1, relsIn->ML, 1, (float*)RELS_tmp_V1); //K*(y(k)-phie'*thetae_1) matrix_addition((float*)relsIn->thetae_1, (float*)RELS_tmp_V1, relsIn->ML, 1, (float*)relsIn->thetae); //thetae(:,k)=thetae_1+K*(y(k)-phie'*thetae_1); //P=(eye(nf+ng+2)-K*phie')*P/lambda; matrix_multiply((float*)relsIn->K, (float*)RELS_tmp_VT1, relsIn->ML, 1, relsIn->ML, (float*)RELS_tmp_M1); //K*phie' 前面已经算过phie'了,这里直接用,前面注意保留 matrix_eye((float*)RELS_tmp_M2, relsIn->ML); //eye(na+nb+1+nc) matrix_minus((float*)RELS_tmp_M2, (float*)RELS_tmp_M1, relsIn->ML, relsIn->ML, (float*)RELS_tmp_M3); //(eye(na+nb+1+nc)-K*phie') matrix_multiply((float*)RELS_tmp_M3, (float*)relsIn->P, relsIn->ML, relsIn->ML, relsIn->ML, (float*)RELS_tmp_M4); //(eye(na+nb+1+nc)-K*phie')*P matrix_multiply_k((float*)RELS_tmp_M4, 1.0/lambda, relsIn->ML, relsIn->ML, (float*)relsIn->P); //P=(eye(nf+ng+2)-K*phie')*P/lambda; // %递推最小二乘法 // phie=[ufk(d:d+nf1);yfk(d:d+ng)]; %这里观测的是除掉△的F // K=P*phie/(lambda+phie'*P*phie); // thetae(:,k)=thetae_1+K*(y(k)-phie'*thetae_1); // P=(eye(nf1+ng+2)-K*phie')*P/lambda; %这里的长度也变了 // //计算观测得到的be0 Fe Ge R // relsIn->BE0 = relsIn->thetae[0][0]; //be0=thetae(1,k); // matrix_multiply_k((float*)relsIn->thetae, 1.0/relsIn->BE0, 1, relsIn->ML, (float*)RELS_tmp_V1); // thetaeb(:,k)=thetae(:,k)/be0; // for(RELS_i=0;RELS_i<(relsIn->NF1+1);RELS_i++){ //F1e=thetaeb(1:nf1+1,k)'; // relsIn->F1E[0][RELS_i] = RELS_tmp_V1[RELS_i][0]; // } // for(RELS_i=0;RELS_i<relsIn->NG+1;RELS_i++){ //Ge=thetaeb(nf1+2:nf1+ng+2,k)'; // relsIn->GE[0][RELS_i] = RELS_tmp_V1[relsIn->NF1+1+RELS_i][0]; // } // // //Fe=conv(F1e,deltaF); %求解带积分项的F 既△F // matrix_init0((float*)RELS_tmp_VT2, 1, PPCFFRELS_ML_A); //初始化为0 // RELS_tmp_VT2[0][0] = 1; //[1 -1] // RELS_tmp_VT2[0][1] = -1; // matrix_init0((float*)relsIn->FE, 1, PPCFFRELS_ML_A); //初始化为0 // fconv((float*)relsIn->F1E, relsIn->NF1+1, (float*)RELS_tmp_VT2, 2, (float*)relsIn->FE); //Fe=conv(F1e,deltaF); // // // matrix_init0((float*)relsIn->R, 1, PPCFFRELS_ML_A); //初始化为0 // //Bm1=sum(Am)/be0; %Bm' // RELS_tmp_U1 = 0; // for(RELS_i=0;RELS_i<relsIn->NAM+1;RELS_i++){ //sum(Am) // RELS_tmp_U1 += relsIn->AM[0][RELS_i]; // } // // // //饮鸩止渴的办法,能缓解非数情况一会儿,但是系统发散的话它是没有任何办法的 // if(relsIn->BE0 < 0.00001 && relsIn->BE0 > -0.00001){ //这里做个强制越界判断,这个数是我随便取得,别太大了,因为b0本来就很小 // relsIn->BE0 = 0.00001; // } // //这个可以应付非数……但效果依旧很差 // // if((int)(relsIn->BE0*1000) == 0){ //这里做个强制越界判断,这个数是我随便取得,别太大了,因为b0本来就很小 // // relsIn->BE0 = 0.00001; // // } // RELS_tmp_U1 /= relsIn->BE0; //Bm1=sum(Am)/be0; // // //R=Bm1*A0; // matrix_multiply_k((float*)relsIn->A0, RELS_tmp_U1, 1, PPCFFRELS_ML_A, (float*)relsIn->R); //一切的一切计算FGR的计算都被移动到这里了 PPCFFRELS_ClcFGR(relsIn); }
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; } }