State observer

Model predictive control requires all the state of the system to be known. All states are, however, rarely measured in practice. Therefore, a state observer that estimates the current states based on measurements is needed. The goal of a state observer is therefore to generate an estimate $\hat{x}$ of the true state $x$, based on measurements $y$ and applied controls $u$ such that $\hat{x} \approx x$

LinearMPC.jl provides a simple steady-state Kalman filter for estimating the states. The steady-state Kalman filter assumes the following model of the dynamical system

\[x_{k+1} = F x_k + G u_k + w_k, \qquad y_k = C x_k + e_k\]

where the process noise $w_k \sim \mathcal{N}(0,Q)$ and the measurement noise $e_k \sim \mathcal{N}(0,R)$. Each time instance, the filter performs two steps: a correction step and a prediction step. In the correction steps, the current state estimate $\hat{x}$ is corrected based on a measurement $y$ according to

\[\hat{x} \leftarrow \hat{x} + K(y-C \hat{x}),\]

which corrects the state based on the deviation between the measurement $y$ and the expected measurement $C\hat{x}$. The correction step is followed by a prediction step, where the state at the next time step is predicted based on the applied control $u$:

\[\hat{x} \leftarrow F \hat{x} + G u.\]

For a MPC struct mpc, a steady state Kalman filter can be created with

set_state_observer!(mpc;Q,R)

where Q and R are covariance matrices for the process noise and measurement noise, respectively. These are used as tuning variables, where the relative size of the elements in Q and R determines if the prediction or the measurements should be trusted more. By default, set_state_observer! uses $F$, $G$, and $C$ from the MPC structure. It is possible to override this by passing the optional arguments F,G,C to set_state_observer!.

Getting, setting, correcting, and predicting state

The current state of the observer is accessed with

x = get_state(mpc)

The state of the observer can be set to x0 with

set_state!(mpc,x0)

Given a measurement y, the state of the observer can be corrected with

correct_state!(mpc,y)

Given a control action u, the next state can be predicted with

predict_state!(mpc,u)

Using the observer

Typically, you want to do a correction step before computing a new control action, and then use the new control action to predict the next state. A typical sequence of calls are therefore

x = correct_state!(mpc,y)
u = compute_control(mpc,x)
predict_state!(mpc,u)

Code generation

If an observer has been set, the codegen function will in addition to C-code for the MPC also generate C-code for the observer. Specifically the C functions mpc_correct_state(state,measurement) and mpc_predict_state(state,control) will be generated, where state, measurement, and control are floating-point arrays. The updated state will be available in the floating-point array state after calling the functions.

Similar to the previous section, the following is a typical sequence of calls when using an observer together with mpc_compute_control

mpc_correct_state(state,measurement)
mpc_compute_control(control,state,reference,disturbance)
mpc_predict_state(state,control)