Esempio n. 1
0
//  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);

}
Esempio n. 2
0
//  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);

}
Esempio n. 3
0
// 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;
}
Esempio n. 4
0
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;
}
Esempio n. 5
0
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;
}
Esempio n. 6
0
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);

}
Esempio n. 7
0
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);

}
Esempio n. 8
0
// 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;
}
Esempio n. 9
0
// 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);
}  
Esempio n. 11
0
/*
 * 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);
}
Esempio n. 12
0
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]; 
} 
Esempio n. 13
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;

    }
}