Low level routines for simulation
For example, let a system defined by: pn = f( pn-1, pn-2, pn-3, input) where p is the variable.
Introducing two variables q and r such as qn= pn-1, rn= pn-1 and a vector X with 3 rows(r,q,p),
the system is described as Xn= F( Xn-1, input), with:
rn= qn-1
qn= pn-1
pn= f( pn-1, qn-1, rn-1, input).
For discrete linear systems, the representation is:
Xn+1= A Xn+ B Un
Yn= C Xn+ D Un
where:
n is the index of the current step/stage.
X is the vector of state variables
Y is the vector of observed variables ( outputs)
U is the vector of inputs, independent from X and Y.
A, B, C, D are matrices independent from X, Y, U.
In most cases, A, B, C, D are independent from n, ie the system is stationnary. This is the assumption made hereunder.
Besides introducing Y will often not be necessary as the output will actually be one of the state variables, ie one element of X.
For a stationary linear system as Xn= A Xn-1+ B U with A constant,
its stability is determined by the roots of its characteristic equation.
The characteristic equation is given by nullifying the
determinant of the matrix A-I*z, I being the identity matrix and z an unknown complex number,
as: det( A-I*z)=0.
Note that this does not depend in anyway on input related terms B and U.
The determinant is a z polynomial with constant real coefficients.
It then can always be decomposed as a product of first and/or second order polynomial(s) with real coefficients.
In our case, we want and are free to choose the roots to fit our stability requirement.
So if we know how to select the root(s) for a first or a second order system, we will be able to
tune a system of any order.
The generic stability analysis process is the following,
starting from a set of numerical equations with unknown constant parameters
1. convert the equations into a first order matricial system
2. compute det( I*z- A) and expand it as a sum of power of z with coefficients.
Unknown parameters will then show in the coefficients of z, z2,z2,...zN.
3.If the max power of z is N, select an appropriate set of first and second order polynomials
so that when all multiplied, the resulting polynomial also has a max power of z of N.
The combination depends on your requirement for damped-oscillating modes or not,
as only second order systems provide those.
4. Set the value of the roots of each of these first and second order systems.
5. Expand their product as a sum of power of z with coefficients.
6. By comparing terms of same power of z with the parametric characteristic equation,
determine the value of its unknown parameters.
The next paragraph deals with what we still miss, ie how to set the value of the roots of first or second order systems.
Xn= -K1 Xn-1- K0 Xn-2+ f( U), with K1, K0 non null constants.
The matricial system is:
Xn= - K1 Xn-1 - K0 XAn-1 + f(U)
XAn= Xn-1
The characteristic equation is (-K1-z)*(-z) - (-K0)*(1)= K0+ K1 z+ z2=0.
The system is stable if the modulus of each of its roots ( which can be complex numbers)
is less than one.
this results in the following constraints:
-1< K0 <1
0< 1+ K0+ K1
0< 1+ K0- K1
To introduce a reference to time, I again set the requirement that the roots correspond
to the root(s) s1 and s2 of the ( stable) continuous system counterpart
d2x/dt2= -k1 dx/dt - k0 x+ g(u).
The roots should then be ( believe me again) exp(T*s1) and exp(T*s2).
1.Assuming the system has a damped-oscillations mode, and introducing the usual set of parameters
, ie the natural frequency ω0 and the damping coefficient ζ
(k0= ω0*ω0 and k1= 2*ζ*ω0)
then K0= exp(-2*T*ω0*ζ) and
K1= -2* exp(-T*ω0*ζ)*cos( T*ω0*sqrt(1-ζ2)).
Note: K1 can be expressed as a function of K0 and ζ as: K1= -2* sqrt(K0)* cos( -0.5* log(K0)* sqrt( 1- ζ2)/ ζ).
Prediction ( reflecting our model of the input):
Ge= Gn-1 ie constant acceleration
Ve= Vn-1+ T* Gn-1 , the estimated speed
Xe= Xn-1+ T* Vn-1+ 0.5* T2 Gn-1, the estimated position
Correction, based on availability of position only (Xc)
Gn= Ge+ C0*( Xcn - Xe )
Vn= Ve+ C1*( Xcn - Xe )
Xn= Xe+ C2*( Xcn - Xe )= (1-C2)* Xe+ C2*Xcn
Getting rid of intermediate variables:
Gn= Gn-1+ C0* ( Xcn- Xn-1- T* Vn-1- 0.5* T2 Gn-1)
Vn= Vn-1+ T* Gn-1+ C1* ( Xcn- Xn-1- T* Vn-1- 0.5* T2 Gn-1)
Xn= (1-C2)*( Xn-1+ T* Vn-1+ 0.5* T2 Gn-1)+ C2* Xcn.
Putting in matricial format, introducing D1= T*C1, D0= 0.5*T2*C0:
Gn= (1-D0)*Gn-1- (C0*T)* Vn-1- C0* Xn-1+ C0* Xcn
Vn= T*(1-0.5*D1)* Gn-1+ (1-D1)*Vn-1- C1* Xn-1+ C1* XCn
Xn= (1-C2)*0.5* T2 Gn-1+ (1-C2)*T* Vn-1+(1-C2)* Xn-1 + C2* Xcn
The matrix (Iz-A) is then, row by row
z+ D0-1; C0*T; C0;
T*(0.5*D1-1); z+D1-1; C1;
(C2-1)*0.5*T2;(C2-1)*T; z+(C2-1);
Computing the determinant from the top row:
det(I*z-A)= (z+ D0-1)*((zD1-1)*(z+(C2-1))+D1*(C2-1))
- T*(0.5*D1-1)*((C0*T)*(z+(C2-1))-C0*(C2-1)*T)
+(C2-1)*0.5*T2*( (C0*T)*(C1)-C0*(z+D1-1))
ie
0= z3
+(D0+ D1+ C2- 3)*z2
+(D0- D1- 2*C2+ 3)*z
+C2-1
D0, D1 and C2 are now left to be defined.
The order of the system is 3, and I choose to have one first order (z-K)
and one second order (K0+K1z+z2)with damped oscillations( instead of 3 first orders).
Choosing k, ω0 and ζ gives K, K0 and K1.
Then I expand to be able to compare with the characteristic equation:
(z-K)*(K0+K1z+z)= z3+ (-K+K1)z2+(K0-K*K1)*z-K*K0
which gives:
D0+ D1+ K2- 3= K1- K
D0- D1- 2*C2+ 3= K0- K1*K
C2- 1= -K0*K
Introducing a=1-K, A=1+K1+K0 and C= 1-K0
C2= 1- K0*K= a+ C- a*C
T*C1= A+a*(C-0.5*A)
T2*C0= A*a
To tune my system,I would set ζ= 0.7 ( good damping), and feed the system with a step input to adjust ω0 whiling keeping m proportional to ω0 to get about the right reaction time( start with k= 2* ω0?), then adjust more precisely ζ, ω0 and k separately, for reaction time, number of visible damped oscillations and overshoot size final adjustments. The final tests would be to use a ramp input and then an input proportional to t2, which should(!) show output= input in steady state.
The prediction equations should reflect the dynamic behavior of the input.
If input follows the simpler Euler scheme, ie
inputn= inputn-1+T* VInputn-1,
then Xe computation must be changed accordingly as Xe= Xn-1+T*Vn-1.
Solving as above gives then:
C2= 1- K0*K, as above.
T*C1= A+a*C.
T2*C0= A*a, as above.
For both filters, if the filter model is consistent with the real input,
G, V and X will be the relevant estimates of GInput, VInput and Input,
and will be respectively equal when the filter is in steady state.
Moreover, a prediction EXn+1 at step n of Xn+1 is available as:
Now, if the filter used is the first one and the input actually
follows a simple Euler integration scheme ( the second model),
G and X will still be equal to GInput and Input in steady state,
but V will differ from VInput, ie VInput- V= -0.5*T*GInput.
The filter will still work as required ( Xn= Xcn in steady state)
, but V will no longer be a relevant estimate of VInput and EXn+1
calculated as above is irrelevant.
The reverse situation would show a difference of +0.5*T*GInput.