void ss_controller::update( matrix *R, struct matrix *Y)
{
matrix_minus(U_tmp, R, X_hat);
matrix_mult(U, K, U_tmp);
for(int i=0; i < num_outputs; i++) {
double* u_i = U->data+i;
double u_max = U_max->data[i];
double u_min = U_min->data[i];
if (*u_i > u_max) {
*u_i = u_max;
} else if (*u_i < u_min) {
*u_i = u_min;
}
}
matrix_mult(b_u, B, U);
matrix_mult(l_y, L, Y);
matrix_mult(l_c, L, C);
matrix_minus(a_lc, A, l_c);
matrix_mult(alc_xhat, a_lc, X_hat);
matrix_add(xhatp1, alc_xhat, l_y);
matrix_add(X_hat, xhatp1, b_u);
}
/* * 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); }
