Bonjour, je suis entrain de programmer un filtre de Kalman en langage C pour la détermination de l'état de charge d'une batterie lithium, mon algorithme marche pour l'instant mais le problème c'est que il met beaucoup de temps pour arriver à la valeur finale, normalement il doit corriger l'erreur à 2 ou 3 pas de calcul,je pense que mon problème vient du gain ou de la matrice de covariance mais j'arrive pas à le régler, j'ai besoin de l'aide svp si quelqu'un à une idée. Merci d'avanceCode:double Rs; // Résistance série double Rst; double Rlt; double Cst; double Clt; double tau_st; double tau_lt; double eta; // Coefficient de courant double C=2.25; // capacité de la cellule double y, x; double Rth=0; // Résistance thermique int n, m,l; #pragma romdata xm // déclaration des tableaux dans leur mémoire respective double xm[3]; #pragma romdata Ak double Ak[3][3]; // Matrice qui relie l'état précédent k-1 à l'état actuel k #pragma romdata Bk double Bk[3]; #pragma romdata Pm double Pm[3][3]; //#pragma udata Ak_t double Ck_t[3][1]; #pragma romdata Ck double Ck[1][3]; #pragma romdata K double K[3]; #pragma romdata T double T[3][3]; #pragma udata Ident double Ident[3][3]; #pragma romdata R double R[3]; #pragma romdata L double L[3]; #pragma romdata Pp double Pp[3][3]; #pragma udata Ew double Ew[3][3]; double Ev; #pragma romdata xp double xp[3]; #pragma romdata S1 double S1[3][1]; double reslt; #pragma romdata S2 double S2[1][3]; #pragma romdata S3 double S3[3]; #pragma romdata S4 double S4[3][3]; double EKF_SOC_init() { Pp[0][0]=100; Pp[0][1]=0; Pp[0][2]=0; // Covariance de SOCinit Pp[1][0]=0; Pp[1][1]=0; Pp[1][2]=0; Pp[2][0]=0; Pp[2][1]=0; Pp[2][2]=0; // on suppose que l'erreur est de 100% Ew[0][0]=23e-6; Ew[0][1]=0; Ew[0][2]=0; // Covariance de bruit de système Ew[1][0]=0; Ew[1][1]=23e-6; Ew[1][2]=0; Ew[2][0]=0; Ew[2][1]=0; Ew[2][2]=23e-6; // Covariance de bruit de mesure xp[0] = 0.5; // SOC init = 0.5 on suppose que l'état de charge est de 50 % à l eta intital xp[1] = 0; // U1 init = 0 xp[2] = 0; Ev=1;// U2 init = 0 return xp[0]; } double EKF_SOC(double tension , double courant, double SOC) { /*déterminasion des paramétres */ /* en cas de charge */ if(courant<0) { Rs = pow(xp[0],2)*0.262-0.2772*xp[0]+0.2204; // Résistance série Rst = (pow(xp[0],2)*0.0029 - 0.0032*xp[0] + 0.0024); // Résistance (short time) Rlt= (pow(xp[0],2)*0.0015 - 0.0016*xp[0] + 0.0012); // Résistance (long time) Cst = (pow(xp[0],2)*(-0.641) + 6.6189*xp[0] + 175.75);// Capacité (short time) Clt = (pow(xp[0],2)*(-0.1219) + 12.175*xp[0] + 296.75);// Capacité (long time) tau_st=Rst*Cst; tau_lt=Rlt*Clt; } else //Décharge { Rs =(pow(xp[0],2)*0.0866-0.1229*xp[0] + 0.2384); Rst = 0.0009*pow(xp[0],2) - 0.0012*xp[0] + 0.0024; Rlt = 0.0004*pow(xp[0],2) - 0.0006*xp[0] + 0.0012; Cst = -103.03*pow(xp[0],2) + 118.28*xp[0] + 187.25; Clt = -127.22*pow(xp[0],2) + 227.83*xp[0] + 262.14; tau_st=Rst*Cst; tau_lt=Rlt*Clt; } eta = 2e-6*pow(courant,2) - 0.0013*courant + 1.0682; /* Matrix A */ Ak[0][0]=1; Ak[0][1]=0; Ak[0][2]=0; Ak[1][0]=0; Ak[1][1]=exp(-dt/(Rst*Cst)); Ak[1][2]=0; Ak[2][0]=0; Ak[2][1]=0; Ak[2][2]=exp(-dt/(Rlt*Clt)); /* Matrix B */ Bk[0]= (-(eta*dt/C)); Bk[1]= Rst*(1-exp(-dt/Rst*Cst)); Bk[2]= Rlt*(1-exp(-dt/Rlt*Clt)); /*Matrice d'identité I*/ Ident[0][0]=1; Ident[0][1]=0; Ident[0][2]=0; Ident[1][0]=0; Ident[1][1]=1; Ident[1][2]=0; Ident[2][0]=0; Ident[2][1]=0; Ident[2][2]=1; /************************* L'état prédit ======> xk+1 = AK*xk+ Bk*uk +Ev **********************/ for (n=0; n<3 ; n++) { L[n]=0; for(m=0; m<3; m++) { L[n] += Ak[n][m]*xp[m]; } } for (n=0; n<3; n++) { xm[n]=0; xm[n]=L[n]+ (Bk[n]*courant); } /* *********************_____prédiction de la matrice de covariance P_____****************************************/ for (n=0; n<3 ; n++) { for(m=0; m<3; m++) { T[n][m]=0; for(l=0; l<3; l++) { T[n][m]+=Ak[n][l]*Pp[l][m]; } } } for (n=0; n<3 ; n++) { for(m=0; m<3; m++) { Pm[n][m]=0; for(l=0; l<3; l++) { Pm[n][m]+=T[n][l]*Ak[l][m]; /*calcul matrice de covariance*/ } } } for (n=0; n<3 ; n++) { for(m=0; m<3; m++) { Pm[n][m]+= Ew[n][m]; } } /** ****************************************calcul de la matrice Gk **** ***** ****** */ // if ( xm[0]<0 ) // { xm[0]=0; // } // if ( xm[0]>=1 ) // { // xm[0]=1; // } x=xm[0]; y =(2.2279*pow(x,5)); y=y - (6.838*pow(x,4)); y=y+ (7.9655*pow(x,3)); y=y-( 3.9534*pow(x,2)) + 1.1628*x + 3.6439; y=y-xm[1]-xm[2]-((Rth+Rs)*courant); Ck[0][0]=((-12e-6)*pow(xm[0],5)); Ck[0][0]= Ck[0][0] +(0.0005*pow(xm[0],4)); Ck[0][0]=Ck[0][0] +(0.00831*pow(xm[0],2))+(0.2698*xm[0])+0.2533; Ck[0][1]= -1; Ck[0][2]= -1; for (n=0; n<1 ; n++) { for(m=0; m<3; m++) { Ck_t[m][n]=Ck[n][m]; /*calcul de la transposé de la matrice C*/ } } /* *****************************____Mise à du gain de kalman____________ ***** **************************** */ /**_______*************K=Pm*Gk_t[Gk*Pm*Gk_t+R]^-1**************** */ /*s1=pm*gkt s2=gk*pm/ s3=s2*Gk_t+R /*calcul de Pm*Gk_t*/ for (n=0; n<1 ; n++) { for(m=0; m<3; m++) { S1[m][n]=0; for(l=0; l<3; l++) { S1[m][n]+=Pm[m][l]*Ck_t[l][n]; } } } for (n=0; n<1 ; n++) { for(m=0; m<3; m++) { S2[n][m]=0; for(l=0; l<3; l++) { S2[n][m]+=Ck[n][l]*Pm[l][m]; } } } for (n=0; n<1 ; n++) { S3[n]=0; for (m=0; m<3 ; m++) { S3[n] += S2[n][m]*Ck_t[m][n]; } } reslt=(S3[0]+Ev); reslt=pow(reslt,-1); //reslt= 1/reslt; for (n=0; n<1 ; n++) { for (m=0; m<3 ; m++) { K[m]=0; K[m]= S1[m][n]*reslt; // k: gain de kalman } } /* estimation de la variable d'état et correction de la prédiction */ for (n=0; n<3 ; n++) { xp[n]= xm[n]+(K[n]*(tension-y)); } /*estimation de l'erreur d'estimation Pp=(I-K*Ck)*Pm */ /* Pp[0][0]=(1-K[0]*Ck[0])*Pm[0][0]; Pp[0][1]=(1-K[0]*Ck[0])*Pm[0][1]+K[0]*Pm[1][0]+K[0]*Pm[2][0]; Pp[0][2]=(1-K[0]*Ck[0])*Pm[0][1]+K[0]*Pm[1][1]+K[0]*Pm[2][1]; Pp[1][0]=(-K[1]*Ck[0]*Pm[0][0]+(1+K[1])*Pm[0][2]+K[1]*Pm[1][2]); Pp[1][1]=(-K[1]*Ck[0]*Pm[0][1]+(1+K[1])*Pm[1][0]+K[1]*Pm[2][0]); Pp[1][2]=(-K[1]*Ck[0]*Pm[0][2]+(1+K[1])*Pm[1][2]+K[1]*Pm[2][1]); Pp[2][0]= -K[2]*Ck[0]*Pm[0][0]+K[2]*Pm[0][2]+(1+K[2])*Pm[1][2]; Pp[2][1]= -K[2]*Ck[0]*Pm[0][1]+K[2]*Pm[1][0]+(1+K[2])*Pm[2][0]; Pp[2][2]= -K[2]*Ck[0]*Pm[0][1]+K[2]*Pm[1][1]+(1+K[2])*Pm[2][1];*/ for (n=0; n<1 ; n++) { S4[n][n]=0; for(m=0; m<3; m++) { S4[n][n]+= K[m]*Ck[n][m]; } } for (n=0; n<3 ; n++) { for(m=0; m<3; m++) { Pp[n][m]=0; for(l=0; l<3; l++) { Pp[n][m] += Pm[n][l]*Ident[l][m]; // Pp[n][m]= Pp[n][m]*Pm[n][m]; } } } for (n=0; n<3 ; n++) { for(m=0; m<3; m++) { T[n][m]=0; //T[n][m]=0; // for(l=0; l<3; l++) // { T[n][m] = S4[0][0]*Pm[n][m]; // Pp[n][m]= Pp[n][m]*Pm[n][m]; // } } } for (n=0; n<3 ; n++) { for(m=0; m<3; m++) { Pp[n][m]=Pp[n][m]-T[n][m]; // Pp[n][m]= Pp[n][m]*Pm[n][m]; } } if ( xp[0]<0 ) { xp[0]=0; // xp[1]=0; // xp[2]=0; } else if ( xp[0]>1 ) { xp[0]=1; } return xp[0]; }
-----
Re : problème avec filtre de KALMAN
Envoyé par ussmag 
Re : problème avec filtre de KALMAN
