Adaptative high-gain extended Kalman filter and applications

tel-00559107, version 1 - 24 Jan 2011
C.3 AEKF Computational Functions C Code
block−>x [1]= block−>x [ 1 ] ∗ block−>x [ 0 ] ;
block−>x [2]= block−>x [ 2 ] ∗ block−>x [ 0 ] ;
}
else if ( f l a g ==1)
{/∗ OUTPUT ∗/
/∗ change o f v a r i a b l e s , normal form c o o r d i n a t e s −> o r i g i n a l ..
c o o r d i n a t e s ∗/
block−>outptr [ 0 ] [ 0 ] = block−>x [ 0 ] ;
block−>outptr [ 0 ] [ 1 ] = block−>x [ 1 ] / block−>x [ 0 ] ;
/∗ Estimated t o r q u e ∗/
block−>outptr [ 0 ] [ 2 ] = block−>x [ 2 ] / block−>x [ 0 ] ;
/∗ High−gain parameter ∗/
block−>outptr [ 0 ] [ 3 ] = block−>x [ n+N ] ;
}
else if ( f l a g ==0)
{/∗ DERIVATIVE ∗/
double p=0,K2=0.068 , J =0.0044 , B=0.0026;
double L=1.22 , Res =5.4183 , K1=0.068;
double DT=0.01 , theta1 =1.25 , lambda=100 , Beta =2000;
double m1=0.005 ,m2=0.004 ,m=m1+m2;
double V=block−>i n p t r [ 0 ] [ 0 ] ;
double Y=block−>i n p t r [ 0 ] [ 1 ] ;
double Inn=block−>i n p t r [ 0 ] [ 2 ] ;
double SI , F0 ;
double z1=block−>x [ 0 ] , z2=block−>x [ 1 ] ;
double z3=block−>x [ 2 ] , t heta=block−>x [ n+N ] ;
double e r r e u r=Y−z1 ;
double Ab [ 9 ] ;
double P11=block−>x [ 3 ] , P21=block−>x [ 4 ] , P31=block−>x [ 5 ] , P22=..
block−>x [ 6 ] , P32=block−>x [ 7 ] , P33=block−>x [ 8 ] ;
double q1=1, q2 =0.1 , q3 =0.01;
if(Yxd [ 0 ] = (V−Res∗z1−K1∗ z2 ) /L+theta ∗ block−>x [ n+0]∗ e r r e u r ; ..
// dz1 / dt =...+P11∗ e r r e u r
block−>xd [ 1 ] = (V∗ z2 /z1−Res∗z2−K1∗ z2∗ z2 / z1 ) /L+(K2∗ z1∗ z1∗z1−B∗..
z2−z3−p∗pow( z2 , 2 . 0 8 ) /pow( z1 , 1 . 0 8 ) ) /J+theta ∗ block−>x [ n..
+1]∗ e r r e u r ; // dz2 / dt =...+P21∗ e r r e u r
block−>xd [ 2 ] = (V∗ z3 /z1−Res∗z3−K1∗ z2∗ z3 / z1 ) /L+theta ∗ block−>x [ n..
+2]∗ e r r e u r ; // dz3 / dt =...+P31∗ e r r e u r
/∗ D e f i n i t i o n o f t h e matrix Ab=(A( u )+b s t a r ) ∗/
Ab[0]=−Res/L ; // b ( 1 , 1 )
Ab[ 1 ] = (K1∗ z2∗z2−V∗ z2 ) /(L∗ z1∗ z1 ) +(3∗K2∗ z1∗ z1 +1.08∗p∗pow( z2 /z1 ..
, 2 . 0 8 ) ) /J ; // b ( 2 , 1 )
Ab[ 2 ] = (K1∗ z2∗z3−V∗ z3 ) /(L∗ z1∗ z1 ) ; // b ( 3 , 1 )
Ab[3]=−K1/L ;
158