2.3 Fast Nonlinear Moving Horizon Estimation
2.3.1 Problem Formulation
As aforementioned, a moving horizon estimator does not consider all past measurements explicitly, but only minimizes the misfit of model prediction and measurements on a fixed estimation horizon of length Tepartitioned into
Neintervals. The intervals are defined by Ne+1 nodes:
t0<t1<. . .<tNe, (2.32)
where t0 = Ti−Te, tNe = Ti; Ti denotes the current time instant. This can
be formulated in form of a constrained, nonlinear least-squares optimization problem. The considered MHE problem formulation in the form of an NLP that is the result of the multiple shooting discretization reads as follows:
min sx, w 1 2 ( ksx0−xack2Qac+ Ne−1
∑
k=0 k h(sxk, uk)− ˜ykk2Wk+kwkk 2 Qw +khNe(sxNe)− ˜yNek2WNe (2.33a) s.t. sx k+1=Fkint(sxk, szk, uk, wk) (2.33b) rk≤r(sxk, uk, wk)≤rk, k=0, . . . , Ne−1 (2.33c) rNe ≤rNe(sxNe)≤rNe. (2.33d)Therein, ˜yk are the measurements and the functions h, hNe are the output
functions commonly referred as to the measurement functions. Typically those functions include sensor models. In addition to the models of the real system outputs, the measurement functions can also include the pseudo-measurement output functions that can help to define a numerically well posed problem. We
FAST NONLINEAR MOVING HORIZON ESTIMATION 31
can notice that in comparison to the MPC formulation the output functions, the IVP solution, and the constraint functions depend on one extra input argument, the disturbance vector w ∈ RnW. In this formulation we do not penalize the
inputs variables uk, although that can be done. The idea to penalize the misfit between the variables ukand the past inputs can be motivated by the fact that
usually the controls computed by e.g. MPC are approximately equal to the ones applied to the real system. The reasons behind this are the finite accuracy and precision of the actuators and/or the fact the connection between the computing hardware and the actuator is realized by analog lines which naturally collect electromagnetic noise. In the formulation at hand, we simply use the past control inputs for simulation purposes.
The optional first term in the least-squares objective is called the arrival cost [33] and is used with the purpose to summarize all information prior to the beginning of the estimation horizon. The a priori estimate is denoted by xac
and the deviation from the variable sx
0 is penalized by the positive definite
matrix Qacthat is the inverse of the a priori covariance matrix. As it was the
case with the MPC formulation, we define the weighting matrices Wk, Qwto
be symmetric positive definite. For brevity, we summarize all possible bounds and constraints on optimization variables in (2.33c) and (2.33d). In contrast to MPC where the constraints are imposed to reflect both physical limitations and design requirements, in estimation the constraints are used for safety, to ensure the physical limitations and/or validity of models are satisfied.
Weighting matrices Within formulation (2.33), it mostly depends on the
choice of the weighting matrices Qac, Wk, Qw, WNe which estimates are con-
sidered optimal. In order to choose them properly, it is common practice to rely on the assumption that the measured quantities exhibit Gaussian distributions. Let us assume that the initial value sx
0is normally distributed random variable
with covariance matrix Σx
0and mean values xest0 . Let also the measured outputs
˜ykand unknown inputs wkbe Gaussian with mean value ykand 0 as well as
covariance matrices Σykand Σw
k, respectively. Then, it is well-known that (2.33)
delivers maximum-likelihood estimates for the true trajectories of the current window if (i) Qacis chosen as(Σx
0)−1, (ii) Wkand Qware chosen as(Σyk)−1and
(Σw
k)−1, respectively, and (iii) no constraints are present. This result has been
extended in [32] to the case where constraints on the estimated quantities are present.
Robust formulations The least-squares objective formulation has one impor-
tant disadvantage. Namely, the presence of outliers, i.e. bad measurements, can significantly deteriorate the estimators performance as the misfits are penalized quadratically. The issue can be simply mitigated by using external logic and declare the measurement missing. However, there exist alternative methods, i.e. objective formulations that can efficiently treat outliers. One popular approach to mitigate the issue is to penalize the misfits with the Hubber norm, where small misfits are penalized with`2norm and the large ones with`1norm [67]. The
norm can be efficient embedded into the Gauss-Newton framework using the algorithmic trick described in [67]. For more sophisticated approaches, such as the M-estimators we refer to [68, 69].
Missing and delayed measurements One of the key advantages of the MHE
approach is that one can handle delayed and missing measurements in an elegant way. The missing measurements are simply handled by setting the appropriate element in the weighting matrix to 0. In statistical sense, this means that one says there infinitely small confidence in the measurement. By suitably choosing the discretization grid, the delayed measurements are simply put on the correct place in the data buffer that is given to the MHE.
Multi-rate sensor fusion Often the system outputs are monitored by the
sensors that output data at different rates. For example, in aerospace applica- tions accelerometer and gyroscope data rates are in order of 1 kHz while the data coming from a GPS is outputted at much lower rates, typically less than 50 Hz. It is desirable to fuse all the data to get the most accurate state estimates. This in turn requires more sophisticated MHE formulations involving multiple discretization grids. Consequently, the sensitivities have to be outputted on all those grid points. The continuous-output MHE was presented in [70], for which the efficient sensitivity generation schemes were previously studied in [44]. For large-scale applications, we refer to [69, 71] and references therein.
Arrival cost approximation The arrival cost term in the objective func-
tion (2.33) summarizes all information gathered through measurements before the beginning of the estimation horizon. The a priori estimate xacis typically
taken from the solution of the MHE problem at the previous estimation instant. The arrival cost matrix Qaccan be chosen in different ways: a constant zero
FAST NONLINEAR MOVING HORIZON ESTIMATION 33
smoothed EKF-update based on sensitivity information obtained while solving the previous MHE problem, which is also what our MHE algorithm uses. It has been shown that the classical EKF is equivalent to MHE using smoothed EKF- updates and a horizon of length one [32]. A similar approach that was found to be suitable for real-time applications is presented in [73]. Approximation of the of the full information estimator has been a topic of extensive research during the past decades. Different approaches have been proposed based different approaches to approximate the arrival cost using either EKF, UKF or particle filters. For an extensive survey on the topic we refer to [19].