# State estimation for ModelingToolkit models

This tutorial will demonstrate how to perform *state estimation* using a model built using ModelingToolkit. State estimation is the process of estimating the state of a system from noisy measurements of the system. For practical systems occurring in engineering and industrial practice, we *almost never* have access to the true state of the system. We may use sensors to measure parts of the state, but sensors are noisy, and it is often impractical to measure all components of the state. Some examples of states that are difficult to measure are

- The state of charge of a battery
- The temperature of the lubrication in a gearbox
- Velocities and angular velocities of moving parts
- In industrial robots, it is common to only be able to measure the angle of a joint on the motor side. The angle of the joint on the other side of the gearbox is often not accessible. The gearbox may be flexible and have backlash, causing the arm-side joint angle to be very different from the motor-side joint angle.
- The internal temperature of the walls in a building.

When designing a controller that uses a model including states that are not measurable, or the measurements are very noisy, we must make use of a state estimator, commonly referred to as a *state observer* in the field of automatic control. A commonly used state observer is the Kalman filter, this is an *optimal* state observer if the system is linear and noise is Gaussian. However, for nonlinear systems, we may have to use a more sophisticated state observer. In this tutorial, we will design an Unscented Kalman filter (UKF) to estimate the position of a 2D mass-spring system. The construction of the system model is detailed in Component-Based Modeling a Spring-Mass System, we reproduce this code below without any details, and then move on to designing the state observer.

## Building the system model

The following code builds the model of the **2D spring-mass system** and simulates it using OrdinaryDiffEq.jl.

```
using JuliaSimControl, ModelingToolkit
using ModelingToolkit, Plots, OrdinaryDiffEq, LinearAlgebra
@variables t
D = Differential(t)
function Mass(; name, m = 1.0, xy = [0., 0.], u = [0., 0.])
ps = @parameters m=m
sts = @variables pos(t)[1:2]=xy v(t)[1:2]=u
eqs = collect(D.(pos) .~ v)
ODESystem(eqs, t, [pos..., v...], ps; name)
end
function Spring(; name, k = 1e4, l = 1.)
ps = @parameters k=k l=l
@variables x(t), dir(t)[1:2]
ODESystem(Equation[], t, [x, dir...], ps; name)
end
function connect_spring(spring, a, b)
[
spring.x ~ norm(collect(a .- b))
collect(spring.dir .~ collect(a .- b))
]
end
spring_force(spring) = -spring.k .* collect(spring.dir) .* (spring.x - spring.l) ./ spring.x
m = 1.0
xy = [1., -1.]
k = 1e4
l = 1.
center = [0., 0.]
g = [0., -9.81]
@named mass = Mass(m=m, xy=xy)
@named spring = Spring(k=k, l=l)
eqs = [
connect_spring(spring, mass.pos, center)
collect(D.(mass.v) .~ spring_force(spring) / mass.m .+ g)
]
@named _model = ODESystem(eqs, t, [spring.x; spring.dir; mass.pos], [])
@named model = compose(_model, mass, spring)
sys = structural_simplify(model)
prob = ODEProblem(sys, [], (0., 2.))
sol = solve(prob, Rosenbrock23())
plot(sol, layout=4, plot_title="Simulation")
```

## State estimation

To build the state observer, we first construct a `FunctionSystem`

from our `ODESystem`

. When doing this, we indicate what our inputs and outputs are. This particular system has no inputs, and we are only able to measure the 2D position of the mass, i.e., we are unable to measure the velocities.

```
model = complete(model)
inputs = []
outputs = collect(model.mass.pos)
funcsys = FunctionSystem(model, inputs, outputs)
p = ModelingToolkit.varmap_to_vars(ModelingToolkit.defaults(model), funcsys.p)
```

```
3-element Vector{Float64}:
1.0
10000.0
1.0
```

The UKF will operate in discrete time with a sample interval $T_s = 5$ms, and the system will be discretized using a 4th order Runge-Kutta method.

```
Ts = 0.005 # Sample time
discrete_dynamics = JuliaSimControl.rk4(funcsys, Ts, supersample=2)
```

The state observer comes from the library LowLevelParticleFilters. This library also contains a function to compute the discrete-time covariance matrices for a double-integrator model, which we will use to pick the dynamics-noise covariance matrix for the system, $R_1$. We also select a measurement-noise covariance matrix $R_2$ and an initial state distribution $d_0$.

The covariance matrices $R_1$ and $R_2$ are defined as

```
using LowLevelParticleFilters
Rdi = LowLevelParticleFilters.double_integrator_covariance(Ts, 1)
R1 = cat(Rdi, Rdi, dims=(1,2)) + 1e-9I
R2 = 0.005I(funcsys.ny)
x0 = sol(0, idxs=funcsys.x)
d0 = MvNormal(x0, R1)
```

We use the simulated solution from above to generate some data to use for state estimation. We add measurement noise to the simulated data to make our experiment more realistic.

```
Tf = sol.t[end] # Final time
timevec = 0:Ts:Tf
u = fill([], length(timevec)) # No inputs
y0 = sol(timevec, idxs=outputs).u # Noise-free output
y = [y0[i] + rand(MvNormal(R2)) for i in 1:length(y0)] # Add measurement noise
```

We are now ready to construct the the state estimator by calling the constructor `UnscentedKalmanFilter`

with our discretized function system. We use the `forward_trajectory`

function to run the state estimation for a full trajectory. The function returns a filtering-solution object which contains the state estimates and the log-likelihood of the solution. We plot the state estimates and the simulated solution together with the noisy measurements.

```
ukf = UnscentedKalmanFilter(discrete_dynamics, R1, R2, d0; p)
filtersol = forward_trajectory(ukf, u, y)
plot(timevec, filtersol, ploty=false, plotx=false, plotu=false)
plot!(sol, idxs=funcsys.x, plot_title="State estimation using UKF")
plot!(timevec, reduce(hcat, y)', sp=[1 3], lab="Measurements", alpha=0.5)
```