Adaptative high-gain extended Kalman filter and applications

tel-00559107, version 1 - 24 Jan 2011
C.4 Innovation Computational Functions C Code
Ab[ 4 ] = ( (V−2∗K1∗ z2 ) /z1−Res ) /L−(B+2.08∗p∗pow( z2 /z1 , 1 . 0 8 ) ) /J ; //..
b ( 2 , 2 )
Ab[5]=−K1∗ z3 /(L∗ z1 ) ; // b ( 3 , 2 )
//Ab [ 6 ] = 0 ;
Ab[7]= −1/J ;
Ab[ 8 ] = ( (V−K1∗ z2 ) /z1−Res ) /L ; // b ( 3 , 3 )
/∗ Computation o f t h e R i c c a t i e q u a t i o n ∗/
block−>xd [ 3 ] = 2 ∗ (Ab [ 0 ] ∗ P11+Ab [ 3 ] ∗ P21 )−theta ∗P11∗P11+theta ∗q1 ;
block−>xd [4]=Ab [ 1 ] ∗ P11+Ab [ 4 ] ∗ P21+Ab [ 7 ] ∗ P31+Ab [ 0 ] ∗ P21+Ab [ 3 ] ∗ ..
P22−t heta ∗P11∗P21 ;
block−>xd [5]=Ab [ 2 ] ∗ P11+Ab [ 5 ] ∗ P21+Ab [ 8 ] ∗ P31+Ab [ 0 ] ∗ P31+Ab [ 3 ] ∗ ..
P32−t heta ∗P11∗P31 ;
block−>xd [ 6 ] = 2 ∗ (Ab [ 1 ] ∗ P21+Ab [ 4 ] ∗ P22+Ab [ 7 ] ∗ P32 )−theta ∗P21∗P21..
+pow( theta , 3 ) ∗q2 ;
block−>xd [7]=Ab [ 2 ] ∗ P21+Ab [ 5 ] ∗ P22+Ab [ 8 ] ∗ P32+Ab [ 1 ] ∗ P31+Ab [ 4 ] ∗ ..
P32+Ab [ 7 ] ∗ P33−t heta ∗P31∗P21 ;
block−>xd [ 8 ] = 2 ∗ (Ab [ 2 ] ∗ P31+Ab [ 5 ] ∗ P32+Ab [ 8 ] ∗ P33 )−theta ∗P31∗P31..
+pow( theta , 5 ) ∗q3 ;
/∗ Computation o f t h e a d a p t a t i o n f u n c t i o n ∗/
SI=1/(1+exp(−Beta ∗( Inn−m) ) ) ;
F0=(thetaxd [9]= SI ∗F0+lambda∗(1− SI ) ∗(1− theta ) ; }
e l s e i f ( f l a g ==5)
{/∗ ENDING ∗/}
}
C.4 Innovation Computational Functions C Code
In the code below, the vector containing the system output past values and, the system
input past values are not updated at the same place.
Let us suppose that we want to compute Id(t). The computation is done via a discrete
process with sample time δ. Therefore we need d
δ +1 values for the output signal (i.e. y(t−d),
y(t − d + δ)),...,y(t)), and d
δ values for the input signal1 (i.e. u(t − d),...,u(t − δ)) in order to
compute a trajectory 2 .
Therefore the computation of innovation at time t requires:
− to update the vector of past values of y from [y(t−d−δ), ..., y(t−δ)] to [y(t−d), ..., y(t)],
− to compute a prediction of the state trajectory with the help of the vectors [y(t −
d), ..., y(t)] and [u(t − d), ..., u(t − δ)],
1 Knowing u(t) is useless here since we don’t want to predict any trajectory for times higher than t.
2 Remember that the initial point of this prediction is the estimated state at time t − d. It is fed to the
function via a delay block.
159