NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Nonlinear Estimation Software Framework inOptimal and Adaptive Control Problems

Miroslav Flıdr, Ondrej Straka and Miroslav Simandl

European Centre of Excellence ”NTIS - New Technologies for the Information Society”

Faculty of Applied Sciences

University of West Bohemia

Flıdr, Straka and Simandl IFAC World Congress 2014 -1/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Introduction to Nonlinear Estimation Framework (NEF)

To provide a software framework designed for nonlinear state estimationof discrete-time stochastic dynamic systems.

The goal of the Nonlinear Estimation Framework (NEF)

í a collection of MATLAB classes and functions formodeling system behaviorstate estimationevaluation of the results

í development driven by the need for a tool that canevaluate the quality of a state estimation method in arbitrary casecompare performance of several state estimatorsprovide means for effortless rapid prototyping of new stateestimators

í can be easily incorporated into adaptive controllerprovides state and parameters conditional probability densityfunction

Flıdr, Straka and Simandl IFAC World Congress 2014 -2/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

The main NEF features

í highly modular and extensibleí designed with support for natural description of the problem in mind

ã enables both the structural and probabilistic description of asystem

ã supports specification of time-varying systemsí implements many of the popular nonlinear state estimators

(both standard and numerically stable estimation algorithms),í fast and easy estimation experiment setupí facilitates implementation of filtering, multi-step prediction and

smoothing tasksí full estimator parametrization by means of the standard MATLAB

property-value mechanism,í evaluation of estimate quality.

Flıdr, Straka and Simandl IFAC World Congress 2014 -3/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

NEF components and estimation experiment description

Model description

Random variablesnefGaussianRV

nefGaussianSumRVnefUniformRVnefEmpiricalRV

nefBetaRVnefGammaRV

nefPDFSystemnefEqSystem

nefHandleFunction

nefLinFunction

nefConstFunction

Functions

EstimatorsnefKalman

nefEnKF

nefSKalman, nefUDKalmannefUSF, nefSUKFnefDD1, nefDD2nefSDD1, nefSDD2

nefSIFnefItFilter

nefGSMnefPF

:

Flıdr, Straka and Simandl IFAC World Congress 2014 -4/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Scheme of NEF modeling component

xk+1 = f k(xk, uk,wk), k = 0, 1, . . . ,zk = hk(xk, uk, vk), k = 0, 1, . . . ,

p(wk), p(vk), p(x0)

Structural description

p(xk+1|xk, uk), k = 0, 1, . . . ,p(zk |xk, uk), k = 0, 1, . . . ,

p(x0).

Probabilistic description

xk ∈ Rnx , zk ∈ Rnz , uk ∈ Rnu ,wk ∈ Rnx , vk ∈ Rnz

Flıdr, Straka and Simandl IFAC World Congress 2014 -5/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Description of functions within NEF

Provided classes for description of multivariate functionsnefHandleFunction general function described by handle functionnefLinFunction linear functionnefConstFunction constant functionnefFunction used by subclassing for complex function

The most useful and common way of describing the functions.Example: h(xk, uk, vk, k) = arctan

(x2,k−sin(k)x1,k−cos(k)

)+ vk

Ê create regular or handle function in MATLABmFun = @(x,u,v,k)...

atan((x(2)-sin(k))/(x(1)-cos(k)))+v

Ë create nefHandleFunction instance with appropriate parametersfun = nefHandleFunction(mFun,[2 0 1 1]);

nefHandleFunction

Flıdr, Straka and Simandl IFAC World Congress 2014 -6/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Structurally and probabilistically described system

Flıdr, Straka and Simandl IFAC World Congress 2014 -7/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Estimation component

í looking for the posterior pdf p(xk |z`, u`)í solution provided by the Bayesian functional relations (BFR)í mostly an approximate solution is being looked forí BFR idea is embodied by class nefEstimator

Estimation problem

Flıdr, Straka and Simandl IFAC World Congress 2014 -8/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Estimators implemented in the NEF estimation component

NEF class estimatorsnefKalman,nefSKalman,nefUDKalman

(extended) Kalman filter (standard, square-rootand UD versions)

nefDD1, nefSDD1,nefDD2, nefSDD2

central difference Kalman filter, divided differ-ence filter (1st and 2nd order) (standard andsquare-root version)

nefUKF, nefSUKF unscented Kalman filter (standard and square-rootversion), cubature Kalman filter

nefItKalman iterated Kalman filternefGSM Gaussian sum filter

nefPFbootstrap filter, generic particle filter, auxiliaryparticle filter, unscented particle filter

nefEnKF ensemble Kalman filternefSIF stochastic integration filter

Flıdr, Straka and Simandl IFAC World Congress 2014 -9/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Estimation tasks supported by individual estimators

estimator filtering prediction smoothingnefKalman X X XnefSKalman X X XnefUDKalman X XnefItKalman X X XnefDD1 X X XnefSDD1 X X XnefDD2 X X XnefUKF X X XnefSUKF X X XnefGSM X XnefPF X XnefEnKF X XnefSIF X X

Flıdr, Straka and Simandl IFAC World Congress 2014 -10/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Performance evaluation

í to measure estimation errorí to compare performance of several estimators against the true value of

the state

Aim of this component

¶ collecting data from Monte Carlo simulations,· extracting appropriate indicators from the conditional distribution of

the state provided by individual estimators¸ evaluating the performance index

Steps to measure performance

Flıdr, Straka and Simandl IFAC World Congress 2014 -11/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Performance indices implemented in the NEF

ABSOLUTE ERROR MEASURESMSEM mean squared error matrixRMSE root mean squared errorAEE average Euclidean errorHAE harmonic average errorGAE geometric average errorMEDE median errorMODE mode error

RELATIVE ERROR MEASURESRMSRE root mean squared relative errorARE average Euclidean relative errorBEEQ Bayesian estimation error quotientEMER estimation error relative to measurement error

PERFORMANCE MEASURESNCI non-credibility indexANEES average normalized estimation error squared

Flıdr, Straka and Simandl IFAC World Congress 2014 -12/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Simple control loop

p(x0|z−1)

Ê Initial condition

method measurementUpdate provides p(xk |zk)

Ê Measurement update step

custom code describing the evaluation of uk

Ë Control law evaluation

method timeUpdate provides p(xk+1|zk, uk)

Ì Time update step

Flıdr, Straka and Simandl IFAC World Congress 2014 -13/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Example of NEF use in control - problem statement

Considered system

xk+1 = Axk + Buk + wk,

zk = Cxk + vk

A =(

0 1θ1 θ2

), B =

(0θ3

),C = (0 1) .

wk ∼ N (0, 0.0001) , vk ∼ N (0, 0.001) .

Criterion

J =E

{N−1∑k=0

(xk+1− xk+1)T Qk+1 (xk+1− xk+1)+ uT

kRk uk

}

Qk+1 =

(0 00 1

), Rk = 0.001.

Flıdr, Straka and Simandl IFAC World Congress 2014 -14/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

LQG controller - problem description using NEF 1/3

f = nefLinFunction(A, B, eye(2));h = nefLinFunction(C,[],1);

Model description

x0=[1;-0.5];Px0=diag(0.2*[1 1]);px0=nefGaussianRV(x0,Px0);

Initial condition

wmean=[0;0]; Pw=eye(2)*0.0001;pw=nefGaussianRV(wmean,Pw);

vmean=0; Qv=0.001;pv=nefGaussianRV(vmean,Pv);

State and measurement noises

model=nefEqSystem(f,h,pw,pv,px0);

Model

UDKalman = nefUDKalman(model);

Estimator (UD Kalman filter)

Note: In this case the parameters θ = (θ1, θ2, θ3) are known!

Flıdr, Straka and Simandl IFAC World Congress 2014 -15/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

LQG controller - implementation 2/3

p(x0|z−1) = N(x0, P0

)Ê Initial condition

µk(uk−10 , yk

0) = xk + K Fk ( yk − Ck xk)

Ê Kalman filter - filtering step

uk(µk) = −K kµk + BT (Fk+1 − Qk+1 xk+1)Ë LQ controller for CE system

xk+1(uk0, yk

0) = Akµk + Bk uk

Ì Kalman filter - predictive step

Flıdr, Straka and Simandl IFAC World Congress 2014 -16/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

LQG controller - implementation 3/3

predPDF.RV=nefGaussianRV(x0,nefUDFactorFunction(Px0));

[z(:,1),x(:,1),model] = simulate(model,1,[],'initialState',x0)

for k = 1:controlHorizon% determine current filtering pdffiltPDF = measurementUpdate(UDKalman,predPDF,[],z(:,k),k);mu(:,k) = evalMean(filtPDF.RV);

% control lawu(:,k)=-(B'*S{k+1}*B+R)\...

(B'*S{k+1}*A*mu(:,k)+B'*F{k+1}-Q*xsetpoint(:,k+1));

% determine one step predictive pdfpredPDF = timeUpdate(UDKalman,filtPDF,u(:,k),k);

% system trajectory simulation[z(:,time+1),x(:,time+1),model] = simulate(model,1,u(:,time));

end

LQG control and trajectory simulation loop using NEF

Flıdr, Straka and Simandl IFAC World Congress 2014 -17/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Cautious controller - setup modifications 1/2

Note: In this case the parameters θ = (θ1, θ2, θ3) are unknown!í the system is bi-linear form estimation point of view

fFun = @(x,u,w,k) [x(2)+w(1);x(3)*x(1)+x(4)*x(2)+x(5)*u+w(2);x(3); x(4); x(5)];

f = nefHandleFunction(fFun,[5 1 2 0]);

Modified state dynamic description for estimation

í it is possible to use nefUDKalman againthe UD factorized extended Kalman filter would be usedit requires specification of first derivative innefHandleFunction constructor

í the nefUKF is better choice

UKF = nefUKF(model);

í control law given as uk = argminukLk (xk, uk)

Flıdr, Straka and Simandl IFAC World Congress 2014 -18/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Cautious controller - implementation 2/2

[z(:,1),x(:,1),model] = simulate(model,1,[],'initialState',x0)for k = 1:controlHorizon% determine current filtering pdffiltPDF = measurementUpdate(UKF,predPDF,[],z(:,k),k);

% extract mean and covariance matrixmu(:,k) = evalMean(filtPDF);Pf = evalVariance(filtPDF);

% construct estimated matrices A and BestA = [ 0 1; mu(3:4,k)'];estB = [ 0; mu(5,k)];

% cautious control lawu(:,k)=-(estB'*Q*estB+Pf(5,5)+R)\...

(estB'*(Q*estA+Pf(3:4,3:4)*mu(1:2,k)-estB'*Q*xsetpoint(:,k+1));

% determine one step predictive pdfpredPDF = timeUpdate(UDKalman,...filtPDF,u(:,k),k);

% system trajectory simulation[z(:,time+1),x(:,time+1),model] = simulate(model,1,u(:,time));

end

Cautious control and trajectory simulation loop using NEF

Flıdr, Straka and Simandl IFAC World Congress 2014 -19/20-

NEF introduction Modeling component Estimation component Performance evaluation NEF in Control problems Conclusion

Concluding remarks

4 versatile tool for testing estimators4 can be easily incorporated into adaptive control law4 problem specification maximally simplified4 provides means even for complex model description4 offers various performance indexes4 possible rapid prototyping of user defined estimators

Recapitulation

í free for non-commercial useí current stable version accessible at http://nft.kky.zcu.cz/

Additional information

Flıdr, Straka and Simandl IFAC World Congress 2014 -20/20-