As I said, since \mathbf{\hat{y}}(k) = \mathbf{h} \big( \mathbf{\hat{x}}_k(k) \big), it is equivalent as bounding the estimated sensor noise \mathbf{\hat{v}}(k) = \mathbf{y}(k) - \mathbf{h} \big( \mathbf{\hat{x}}_k(k) \big) in the MHE. I will move the definition of \mathbf{\hat{V}} and \mathbf{\hat{W}} vectors in the extended help of MovingHorizonEstimator constructor to clarify that point.
I found this great lecture that mentions bounds on \mathbf{\hat{x}}, \mathbf{\hat{v}} and \mathbf{\hat{w}}. I will implement the three types of bounds. It will probably leads to infeasibilities in some case so I will also add soft constraints later.
The MovingHorizonEstimator types now support hard constraints on the estimated state \mathbf{\hat{x}}, process noise \mathbf{\hat{w}} and sensor noise \mathbf{\hat{v}}. Additionally, MHEs based on LinModels are now treated as quadratic programs (QP) solved with OSQP.jl by default instead of nonlinear problems. From my quick tests, it’s about 500 times faster than v0.14! I never did the exercise for the MHE, and it was surprisingly a long and hard job to construct all the prediction matrices that appear in the quadratic objective and the constraints . I was also surprised when I understood that, contrarily to LinMPC, the Hessian matrix of the QP is not constant, because of the time-varying value of the arrival covariance \mathbf{\bar{P}}. This means that solvers need to re-factorize the QP matrices at each discrete time k. This is still drastically cheaper than treating the optimization problem as nonlinear.
The changelog is:
added: MHE hard constraints on estimated process and sensor noises
added: MHEs based on LinModel are now treated as a quadratic optimization instead of nonlinear
added: Plots recipe avoid repeat xlabels for MIMOs
added: getinfo method for MovingHorzionEstimator for troubleshooting
debug: update MHE arrival covariance with I/Os at arrival
improved coverage
The next step is supporting soft constraints for the MHE.
I added the support for soft-constraints in the MovingHorizonEstimator. I chose to disable this feature by default for two reasons:
most people will only constraints the state estimate. It should not lead to major infeasibility if the bounds are correctly chosen.
from my tests, soft-constraining NonLinModel estimations leads to problems that Ipopt has difficulties to solve, because of the weird scaling of the decision variables (no issue with LinModel and OSQP tho). Indeed, with the C weight around 10^5, the solution for \epsilon is typically around 10^{-5}, while the other decision variables follow the scale of the states, generally larger in magnitude. Reducing C and nlp_scaling_max_gradient option of Ipopt can mitigate this.
The changelog is:
added: MHE soft constraints with a single slack variable ϵ (a.k.a. equal concern for relaxation, disabled by default thus only hard constraints).
changed: more concise MPC logging in REPL.
removed: deprecated arguments for setconstraint! and NonLinMPC.
doc: hide extended help with details admonition.
doc: more concise setconstraint! and ExplicitMPC documentation.
I will try to add an example with the MHE in the manual.
It’s been a while since my last post! Many new features were added since then, with a breaking change: NonLinModel constructor now supposes continuous dynamics by default. Use solver=nothing for discrete-time models.
Here is the changelog since my last update:
added: support for NonLinModel with continuous dynamics
added: 4th order Runge-Kutta solver with 0 allocation
added: support for mutating NonLinModel (in-place) to reduce the allocations
added: support for non-diagonal M_Hp, N_Hc and L_Hp weight, allowing terminal costs and LQR tuning
doc: simple MHE example on the CSTR (manual)
doc: info on terminal weights, continuous NonLinModel and mutating syntax
doc: the pendulum example now use the built-in Runge-Kutta solver
added some tests with continuous NonLinModel, both mutating and non-mutating
Also, the package is now part of JuliaControl organization! Thanks @baggepinnen for the help!
BTW @odow is there any timeline for when NLopt.jl will support the new NLP syntax ? I finished the migration but I would like at least two FOSS NLP solvers that works (i.e. other than Ipopt.jl) before merging into main.
I’m super excited to announce two new major features:
linear model adaptation of controller/estimator at runtime
model linearization at non-equilibrium points
These features allow adaptive MPC based on online parameter estimation (e.g. recursive system identification). Furthermore, successive linearization MPC can now be implemented with minimal effort. It’s a great feature since e.g. MATLAB also supports it, but the analytical expressions of the Jacobians are required (hidden in the “Successive Linearizer” block). This is not the case here with the AD-based linearize function. Also, similar functionalities are available for the MovingHorizonEstimator.
A massive and long refactoring of the operating points and the mathematical notation was required to support linearization at non-equilibrium points. It was totally worth it since the successive linearization MPC on the pendulum is about 125 times faster than the NMPC counterpart, with similar closed-loop performances! It’s a great application of Julia’s AD tools and its first class linear algebra library!
BREAKING CHANGE
All the keyword arguments related to initial values e.g. σP0, x0 and x̂0 now require an underscore e.g. σP_0, x_0, x̂_0 (to clearly differentiate from deviation vectors around operating points)
Added: setmodel! for runtime model adaptation of controller/estimator based on LinModel
Added: linearize and setop! now support non-equilibrium points
Added: successive linearization MPC with the new setmodel! and linearize functions
Added: successive linearization MHE with the new setmodel! and linearize functions
Added: linearize! method for in-place model linearization (to reduce allocations)
Added: 6 args. LinModel constructor now support scalars (similarly to ss function)
Added: ExtendedKalmanFilter now compute the Jacobians in-place (to reduce allocations)
Changed: struct state data x and state estimate x̂ renamed to x0 and x̂0
Debug: ExplicitMPC with non-Float64 now works
Debug: accept integers in linearize arguments
Debug: call empty! on JuMP.Model to support re-construction of MPC instances
Doc: new setmodel!, setop! and linearize function documentation
Doc: example of model adaptation with successive linearization on the pendulum (very efficient!)
other changes, since my last post on discourse:
Added: print info on controller and estimator constraint softening (slack var. ϵ)
Added: custom estimator for the approximation of the arrival covariance in the MHE
Added: improve performance of LinMPC and MHE with new JuMP batch update methods
Various allocation reductions
Changed: MHE now default to an UnscentedKalmanFilter for the arrival covariance estimation (instead of an EKF)
Debug: MHE with NonLinModel update covariance with the correct state estimate
One MPC problem that I’ve never seen anyone really address…
MPC algorithms tend to assume that each optimization takes zero time, in other words: that the computed optimal solution u*[t_k] for u[t_k] can be injected into the system at time t = t_k.
In practice, computation takes some time, so the computed value u*[t_k] is actually injected at a delayed time t = t_k+tau, i.e., u[t_k+tau] = u*[t_k], while for t_k-1 + tau < t < t_k+tau, u[t] = u*[t_k-1]
In other words: in real life, MPC adds a time delay to the system – an this time delay may actually vary – depending on constraints in the system, current state, etc. This may or may not be a problem.
One way around this is to assume a maximum time delay and then include this time delay in the model so that the computation of u*[t_k] already is aware of this time delay.
Anyways, I don’t know whether your MPC package can address such problems?
The approach of modeling the computational delay as an input delay in the plant is of course available, even if the delay is a fraction of a sample (using Thiran approximation). When using linear MPC, neglecting the computational delay is not always such a bad idea, it takes a couple of microseconds to solve even reasonably large problems (in state dimension and prediction horizon). For computationally more demanding problems, real-time iteration (RTI) is often worthwhile, this is something that isn’t yet supported, but would be good to get in there eventually. With RTI, you don’t bother iterating each problem until convergence, and instead favor to get new measurement data at a higher rate such that later iterations are made using the latest available data. The reasoning being that performing optimizer iterations with “old data” is wasteful if new data is available.
I agree on linear MPC, although it may depend on whether you allow for hard constraints on inputs, and more importantly on outputs.
Already 20+ years ago, Biegler and co-workers solved linear MPC with 200 inputs and 800 outputs (?) in … 10 or 100 ms or so; they considered only input constraints.
So if one has n states and a horizon of N, this gives in the order of n\cdot N unknowns (+ inputs etc., etc.). It then probably is important to take advantage of the structure of the resulting problem. Without constraint, this can be solved by handling the n\cdot N \times n\cdot N matrix involved (ok: really larger, by including inputs, outputs, etc.). Alternatively, one can instead solve this using the Riccati-equation formulation, which reduces the complexity to handling N problems of size n\times n. At the outset, this only works for unconstrained problems. But I seem to recall that in a PhD thesis (or licenciate thesis?) from Uppsala (?) some 15-20 years ago, someone (don’t recall the name) reformulated linear quadratic problems with input constraints into a Riccati-like form. [This could also be interesting for receding horizon state estimation.]
With output constraints, QP type algorithms are needed, which may slow the solution down considerably if the problem in certain operating points is infeasible or near infeasible. Output constraints are important in some applications, e.g., electricity generation where there are (semi-) hard constraints on grid frequency, etc.
But if the system is nonlinear, or worse: distributed (spatial distribution, population balance, etc.), the problem I raise is more interesting. Of course, for systems that are slow to solve, in practical implementations, one may resort to surrogate models, etc.
A quick update on the last improvements. I first updated the internal code to the new NLP syntax of JuMP.jl. It is a slightly breaking change for some custom optimizers. There is only NLopt.jl that does not support it yet. I tested Ipopt.jl, MadNLP.jl and KNITRO.jl and they work well.
Also, following the model adaptation feature, it is now possible to modify the weight matrices in the objection function at runtime using setmodel!. Same thing is possible with the covariance matrices of the Kalman Filter and moving horizon estimator.
Updates on the new release of the package. There are some breaking changes in the API related to important new features.
All the state estimator in the package were previously implemented using the predictor form (a.k.a. delayed form), meaning that they estimated the state of the next time step, denoted \mathbf{\hat{x}}_{k}(k+1) in this package, or \mathbf{\hat{x}}_{k+1|k} elsewhere. They are a bit simpler to use, and they allow moving the computations of the estimator after the MPC optimization (thus reducing the time delay mentioned by @BLI in the posts above, especially with the MHE).
But, as raised by @baggepinnen (and thanks for the suggestion!), they introduce a one sample time delay in the control loop. The closed-loop robustness is thus slightly affected by them. They are also a bit less accurate than the current form (a.k.a filter form).
Because of that, I added the support for the current form that estimates \mathbf{\hat{x}}_{k}(k) on all the StateEstimator in the package, with the exception of the MovingHorizonEstimator for now, since the implementation is slightly more challenging. There is a new direct keyword argument (same as kalman and place in ControlSystems.jl) on all StateEstimator (except InternalModel since direct=false is impossible). The default value is true, referring to this new current form. You can notice that disturbance rejection response is indeed improved in the examples of the manual. BTW, the steady-state Kalman filter in the current form (the new default) is exactly the same approach as MATLAB.
A new preparestate! function must be called before solving the MPC problem with moveinput!. Note that this function does nothing if direct==false. This allow writing you own simulation for loop with e.g. disturbances, and changing the estimator type or form “on-the-fly” in the constructor. The code inside the for loop will still work without any change. Also note that an error will be thrown is the user forget to call preparestate! before updatestate!. See the test_mpc function in Linear Model Predictive Control section of the manual for an example of the new API.
preparestate! should be called before solving the MPC optimization with moveinput!
current form by default for all StateEstimator
ym keyword argument of moveinput! removed (was only for InternalModel, I now use the new preparestate! method)
MovingHorizonEstimator advanced constructor (with complete covariance matrices): covestim and optim are now keyword arguments instead of positional arguments.
Changelog
added: current form for all StateEstimator except MovingHorizonEstimator, to improve accuracy and closed-loop robustness
reduce allocation for SimModeland StateEstimator instances by using internal buffers
A quick update on the new release. I worked hard on adding the current/filter formulation of the MovingHorizonEstimator (the direct keyword argument, corresponding to p=0 in the equations). It was not easy since this form of the MHE is kind of overlooked in the literature. AFAIK, I only found http://facta.junis.ni.ac.rs/acar/acar201202/acar20120202.pdf that discuss a little bit more in detailed about this formulation (thank you prof. Morten Hovd!). I documented all the internal computations in the Extended Help of this function. Like all the other state estimators, the default is now this new formulation, which is a slight breaking change.
The docstring of all the state estimator are also now more detailed in many ways. I explicitly added the keyword arguments, instead of referring to SteadyKalmanFilter or KalmanFilter documentation, at the risk of being repetitive.
I also added some simple soft real time utilities. It support both idle sleep and busy-wating, intended for higher frequency sampling.
The changelog is:
breaking change: MovingHorizonEstimator now default to direct=true
added: the current/filter formulation of the MovingHorizonEstimator
doc: explicitly list all the keyword arguments of all the state estimator
added: soft realtime utilities
added: InternalModel now produces 0 allocation with preparestate! and updatestate! calls
tests: new integration tests that compare unconstrained MHE to UKF and KF results
tests: new integration that compare LinModel and NonLinModel
and, since my last post:
Luenberger, SteadyKalmanFilter, KalmanFilter and UnscentedKalmanFilter now produce 0 allocation with preparestate! and updatestate! calls
This release marks a final breaking change to initiate the first major version!
I don’t plan for any other breaking change in the next releases. I feel that most essential features are there, and the public interface is flexible enough to accommodate many types of predictive controllers and state estimators.
Two new features are introduced:
plant model parameters \mathbf{p}
custom economic function J_E parameters \mathbf{p} (equal to the model parameters by default).
A closure was needed for that in the past. This can be dangerous on the performance and type stability, especially when working in the global scope. Now closures can completely avoided.
A new example is also added to the manual: the pendulum model integrated into ModelingToolkit.jl (MTK). A controller is designed and tested from this model to reproduce the results of the previous section in the manual. Note this is not an official interface with MTK and the provided code is not guaranteed to cover all corner cases.
. I was also surprised when I understood that, contrarily to 
Thanks 
Updates on the new release of the package. There are some breaking changes in the API related to important new features.
