Conditionally Linear Systems

Conditionally linear systems are a special class of nonlinear systems where the dynamics can be partitioned into nonlinear and linear substates. This structure enables significant computational savings in state estimation and control design.

Theory

Consider a nonlinear dynamical system:

\[\dot{x} = f(x, u)\]

where $x \in \mathbb{R}^{n}$ is the full state vector and $u$ is the control input.

A system is conditionally linear if the state $x$ can be partitioned into two substates:

• $x^n \in \mathbb{R}^{n_n}$: the nonlinear substate
• $x^l \in \mathbb{R}^{n_l}$: the linear substate

such that the dynamics can be written in the form:

\[\dot{x} = f(x^n, u) + A(x^n, u)x^l = \begin{aligned} \dot{x}^n &= f_n(x^n, u) + A_n(x^n, u) x^l \\ \dot{x}^l &= f_l(x^n, u) + A_l(x^n, u) x^l \end{aligned}\]

In this representation:

• The nonlinear substate $x^n$ may appear nonlinearly
• The linear substate $x^l$ appears only linearly (affinely)
• The matrices $A_n$ and $A_l$ may depend on $x^n$ and $u$, but not on $x^l$

This structure is useful for efficient state estimation using marginalized filters, such as the Marginalized Unscented Kalman Filter (MUKF).

Partitioning Algorithm

The conditionally_linear function uses a greedy heuristic to partition the state vector:

Step 1: Individual Analysis

First, identify variables that must be in the nonlinear substate $x^n$ because they appear non-affinely when considered individually.

Step 2: Pairwise Conflict Graph

Among the remaining variables, build a "conflict graph" where an edge between variables $v_i$ and $v_j$ indicates that they cannot both be in $x^l$ (they appear jointly in a nonlinear way, e.g., through a product $v_i \cdot v_j$).

Step 3: Greedy Selection

Greedily construct $x^l$ by adding variables with the fewest conflicts first, verifying at each step that the current $x^l$ remains affine.

Preference Hints

You can guide the partitioning by providing a preference dictionary that assigns priority values to variables. Variables with lower preference values are preferred for inclusion in $x^l$. This is useful when you have domain knowledge about which variables:

• Vary slowly (parameters)
• Have more accurate dynamics models
• Are computationally expensive to include in the nonlinear substate

Connection to State Estimation

The conditionally linear structure is particularly valuable for state estimation using the Marginalized Unscented Kalman Filter (MUKF). When a system has conditionally linear structure, MUKF can:

• Reduce sigma points: For a system with $n_n$ nonlinear dimension and $n_l$ linear dimension, MUKF uses only $2n_n + 1$ sigma points instead of $2(n_n + n_l) + 1$ that a standard UKF would require
• Enable deterministic estimation: Unlike particle filters, MUKF is deterministic and suitable for gradient-based hyperparameter optimization

Example: Quadrotor with Unknown Parameters

Consider a quadrotor where mass $m$ and drag coefficient $C_d$ are unknown. By using the reparameterization $\theta = 1/m$ and $\varphi = \theta C_d$, the system becomes conditionally linear:

Nonlinear substate: velocities $[v_x, v_y, v_z]$Linear substate: positions $[x, y, z]$, inverse mass $\theta$, and scaled drag $\varphi$

This reduces sigma points from 17 (for 8D UKF) to 7 (for 3D nonlinear MUKF), a 59% reduction!

For a complete example, see the MUKF parameter estimation documentation in LowLevelParticleFilters.jl.

Implementation in DyadControlSystems

DyadControlSystems provides the conditionally_linear function to automatically identify the conditionally linear structure of a system.

Examples

Example 1: Simple System with Bilinear Terms

This example demonstrates a system where some variables are individually affine but have pairwise conflicts due to bilinear terms.

using DyadControlSystems
using Symbolics

@variables x1 x2 x3 u[1:1]
x = [x1, x2, x3]
u = collect(u)

# System dynamics
rhs = [
    -x[1]                       # x1dot: linear in x1
    2x[1]*x[2] + sin(x[2])      # x2dot: nonlinear (x1*x2 product and sin)
    2x[2]                       # x3dot: linear coupling with x2
]

# Partition into nonlinear and linear substates
res = conditionally_linear(rhs .+ [exp(2x[2]); 0.5; 0;;]*u, x)
(; xn, xl, f, A) = res

println("Nonlinear substate: ", xn)  # Expected: [x2]
println("Linear substate: ", xl)     # Expected: [x1, x3]

Nonlinear substate: Symbolics.Num[x2]
Linear substate: Symbolics.Num[x1, x3]

The function returns:

• xn: Nonlinear substate variables
• xl: Linear substate variables
• f: Drift term $f(x^n)$
• A: Coupling matrix $A(x^n)$ such that $\dot{x} = f(x^n) + A(x^n) x^l$

Example 2: Complex System with Multiple Couplings

@variables x1 x2 x3 x4 x5 x6 x7 x8 u[1:3]
x = [x1, x2, x3, x4, x5, x6, x7, x8]
u = collect(u)

# Complex dynamics with mixed linear and nonlinear couplings
rhs = [
    -x[1] + cos(x[1]) + x[2]*x[3] + exp(x[5])*u[1];      # Nonlinear in x1, x2, x3, x5
    x[1]*x[2] + tanh(x[3]) + u[2]*x[5];                  # Nonlinear
    x[2]^2 + sin(x[3]) + u[3];                           # Nonlinear
    (1 + x[2]^2)*x[4] + x[1] - u[1];                     # Linear in x4 (xn-dependent gain)
    exp(x[1]) + x[2]*x[5] - u[2];                        # Nonlinear
    (2 + sin(x[3]))*x[6] + x[4] + u[3];                  # Linear in x6
    (u[1] + x[1]^2)*x[7] + x[6] - x[8] + x[2];           # Linear in x7, x8
    (x[2] + u[2]^2)*x[8] + x[7] + x[3]                   # Linear in x8
]

res = conditionally_linear(rhs, x)
(; xn, xl, f, A) = res

println("Nonlinear substate: ", xn)  # [x1, x2, x3, x5]
println("Linear substate: ", xl)      # [x4, x6, x7, x8]

Nonlinear substate: Symbolics.Num[x1, x2, x3, x5]
Linear substate: Symbolics.Num[x4, x6, x7, x8]

Example 3: Pairwise Conflicts

This example shows how the algorithm handles variables that are individually affine but jointly non-affine:

@variables x1 x2 x3 x4 x5 x6 u[1:2]
x = [x1, x2, x3, x4, x5, x6]
u = collect(u)

rhs = [
    sin(x[1]) + x[2]^2 + u[1];              # x1, x2 clearly nonlinear
    exp(x[2]) - u[2];                        # x2 nonlinear
    (1 + x[1]^2)*x[3] + x[4] - u[1];        # x3 individually affine
    (2 + cos(x[1]))*x[4] + x[5];            # x4 individually affine
    x[6] + u[1] - 3x[5];                    # x5, x6 affine
    x[3]*x[4] + x[5] - x[6] + u[2]          # ← Bilinear term x3*x4!
]

res = conditionally_linear(rhs, x)
(; xn, xl, f, A) = res
# Result: x3 and x4 cannot both be in xl due to the x3*x4 term
# The algorithm will place one in xn and one in xl based on conflict analysis
println("Nonlinear substate: ", xn)

preference = Dict(x[3] => 2, x[4] => 1)
# With preference_first=true, preference takes priority over conflict count, a smaller preference number makes the variable likely to be picked first
res = conditionally_linear(rhs, x; preference=preference, preference_first=true)
(; xn, xl, f, A) = res
println("Nonlinear substate with preference: ", xn)

Nonlinear substate: Symbolics.Num[x1, x2, x4]
Nonlinear substate with preference: Symbolics.Num[x1, x2, x3]

Use preferences when you know that certain variables:

• Represent slowly-varying parameters
• Have more reliable dynamics models
• Are computationally expensive in the nonlinear substate

Example 4: State Estimation with MUKF - Quadrotor with Unknown Parameters

This comprehensive example demonstrates the full workflow from defining a ModelingToolkit system to performing state estimation with the Marginalized Unscented Kalman Filter (MUKF). We'll estimate the mass and drag coefficient of a quadrotor while tracking its trajectory.

The Challenge

Consider a 3D quadrotor with unknown mass $m$ and drag coefficient $C_d$. The naive approach would be to augment the state with these parameters and use an 8-dimensional UKF, requiring 17 sigma points. However, with a clever reparameterization, we can expose conditionally linear structure and reduce this to just 7 sigma points (59% reduction)!

The Clever Trick: Reparameterization

Instead of directly estimating $m$ and $C_d$, we reparameterize:

• $\theta = 1/m$ (inverse mass)
• $\varphi = \theta C_d = C_d/m$ (mass-scaled drag)

This makes the dynamics conditionally linear: velocities appear nonlinearly, but positions and the parameters appear linearly conditioned on the velocities.

Implementation

using DyadControlSystems
using ModelingToolkit
using LowLevelParticleFilters
using SeeToDee
using LinearAlgebra
using StaticArrays
using Plots

# Define the quadrotor system with clever parameterization baked in
t,D = ModelingToolkit.t_nounits, ModelingToolkit.D_nounits

@mtkmodel Quadrotor begin
    @parameters begin
        g = 9.81  # Gravity
    end
    @variables begin
        # Velocities (nonlinear substate)
        vx(t) = 0.0
        vy(t) = 0.0
        vz(t) = 0.0

        # Positions (linear substate)
        x(t) = 0.0
        y(t) = 0.0
        z(t) = 0.0

        # Reparameterized parameters (linear substate)
        θ(t) = 1.0    # θ = 1/m (inverse mass), true value = 1.0 for m=1.0 kg
        φ(t) = 0.01   # φ = θ*Cd = Cd/m (scaled drag), true value = 0.01 for Cd=0.01

        # Control inputs (forces in each direction)
        Fx(t)
        Fy(t)
        Fz(t)
    end

    @equations begin
        # Velocity dynamics: forces with quadratic drag (conditionally linear!)
        # dv/dt = θ*F - φ*v|v| - [0,0,g]
        # The key: v*|v| is nonlinear in v, but θ and φ enter linearly
        D(vx) ~ θ*Fx - φ*vx*abs(vx)
        D(vy) ~ θ*Fy - φ*vy*abs(vy)
        D(vz) ~ θ*Fz - φ*vz*abs(vz) - g

        # Position dynamics (linear)
        D(x) ~ vx
        D(y) ~ vy
        D(z) ~ vz

        # Parameter dynamics (random walk model for slowly varying parameters)
        D(θ) ~ 0
        D(φ) ~ 0
    end
end

@named model = Quadrotor()
model = complete(model)

# Partition into conditionally linear structure
inputs = [model.Fx, model.Fy, model.Fz]
outputs = [model.vx, model.vy, model.vz, model.x, model.y, model.z]

Ts = 0.01  # Sampling time

result = conditionally_linear(model, inputs, outputs; split=false, Ts)
println(result)
println("\nNonlinear substate (velocities): ", result.f_result.xn)
println("Linear substate (positions + parameters): ", result.f_result.xl)

# Build compiled functions for MUKF
p = ModelingToolkit.get_p(result.sys, [inputs .=> 0; ])
dynamics_functions, output_functions = Symbolics.build_function(result)

# Extract state indices for MUKF
nx = dynamics_functions.nx
ny = length(outputs)
nu = dynamics_functions.nu

#### Simulation Setup

# Create true system for data generation (with time-varying parameters!)
function true_dynamics_continuous(x, u, p, t)
    # True parameters change over time (realistic scenario)
    # Mass decreases linearly (fuel drain), then stays constant
    m_true = t < 10.0 ? 1.0 - 0.015*t : 0.85
    # Drag increases abruptly at t=10s (damage/configuration change)
    Cd_true = t < 10.0 ? 0.01 : 0.015

    vx, vy, vz, x_pos, y_pos, z_pos = x[1:6]
    Fx, Fy, Fz = u
    g = 9.81

    # True dynamics with quadratic drag: dv/dt = F/m - Cd*v|v|/m - g_z
    dvx = Fx/m_true - Cd_true*vx*abs(vx)/m_true
    dvy = Fy/m_true - Cd_true*vy*abs(vy)/m_true
    dvz = Fz/m_true - Cd_true*vz*abs(vz)/m_true - g

    dx = vx
    dy = vy
    dz = vz

    return [dvx, dvy, dvz, dx, dy, dz]
end

assumed_state_order = [result.sys.vx, result.sys.vy, result.sys.vz, result.sys.x, result.sys.y, result.sys.z, result.sys.θ, result.sys.φ]

assumed_to_actual = [findfirst(isequal(var), result.x) for var in assumed_state_order]

# Discretize using Rk4 (high-order accurate)
true_dynamics_discrete = SeeToDee.Rk4(true_dynamics_continuous, Ts; supersample=4)

# Generate control inputs (figure-8 trajectory)
T = 20.0  # Simulation time
times = 0:Ts:T
N = length(times)

u_traj = zeros(nu, N)
for i in 1:N
    t = times[i]
    # Control inputs for figure-8 motion
    u_traj[1, i] = 5.0*cos(0.5*t)  # Fx
    u_traj[2, i] = 3.0*sin(1.0*t)  # Fy
    u_traj[3, i] = 9.81 + 2.0*sin(0.3*t)  # Fz (compensate gravity + variation)
end
u_traj .*= 10

# Simulate true trajectory
x_true = zeros(6, N)
x_true[:, 1] = [0.0, 0.0, 0.0, 0.0, 0.0, 10.0]  # Initial state (start at z=10m)

for i in 1:N-1
    x_true[:, i+1] = true_dynamics_discrete(x_true[:, i], u_traj[:, i], [], times[i])
end

# Generate noisy measurements
measurement_noise = 0.1
y_meas = x_true .+ measurement_noise * randn(size(x_true))

#### MUKF Setup
# Process noise covariance
R1 = 100*SMatrix{nx,nx}(Diagonal([
    Ts, Ts, Ts,   # Velocity process noise
    Ts^3/3, Ts^3/3, Ts^3/3,      # Position 
    10Ts^3,              # θ process noise (slow variation)
    0.1Ts^3              # φ process noise (slow variation)
])[assumed_to_actual, assumed_to_actual])  # Reorder to match assumed state order

# Measurement noise
R2 = SMatrix{ny,ny}(Diagonal(fill(measurement_noise^2, ny)))

# Initial state distribution
x0 = SVector{nx}([
    0.0, 0.0, 0.0,      # velocities (guess zeros)
    0.0, 0.0, 10.0,     # positions (initial position)
    0.9,                # θ initial guess (true starts at 1.0)
    0.008               # φ initial guess (true starts at 0.01)
][assumed_to_actual])  # Reorder to match assumed state order

R0 = SMatrix{nx,nx}(Diagonal([
    0.1, 0.1, 0.1,      # Velocity uncertainty
    0.1, 0.1, 0.1,      # Position uncertainty
    0.1,                # θ uncertainty
    0.01                # φ uncertainty
])[assumed_to_actual, assumed_to_actual])  # Reorder to match assumed state order

d0 = LowLevelParticleFilters.SimpleMvNormal(x0, R0)

# Create measurement model
mm = LowLevelParticleFilters.RBMeasurementModel(output_functions.f, R2, ny)

names = SignalNames(x = string.(result.x), u = string.(inputs), y = string.(outputs), name="MUKF")
# Create MUKF (all keyword arguments!)
mukf = LowLevelParticleFilters.MUKF(;
    dynamics = dynamics_functions.f,
    nl_measurement_model = mm,
    dynamics_functions.A,
    Cl = output_functions.A,
    R1,
    d0,
    nxn = dynamics_functions.nxn,
    nu = dynamics_functions.nu,
    ny,
    Ts,
    dynamics_functions.n_inds,
    dynamics_functions.l_inds,
    p,
    names,
)


U = SVector{nu}.(eachcol(u_traj))
Y = SVector{ny}.(eachcol(y_meas))
# predict!(mukf, U[1])
# correct!(mukf, U[1], Y[1])

# Run forward trajectory
sol = forward_trajectory(mukf, U, Y)
plot(sol, plotx=false, plotxt=false, plotu=false)
plot(sol, plotx=true, plotxt=true, plotu=false, ploty=false, plotyh=false, size=(800,800), plotRt=true)

##
# Extract state estimates (convert from vector of vectors to matrix)
actual_to_assumed = invperm(assumed_to_actual)  # Inverse mapping
x_est = reduce(hcat, sol.xt)'[:, actual_to_assumed]

#### Visualization

# Plot results
plot_result = plot(layout=(5,1), size=(800, 1000))

# Plot 1: XYZ trajectory
plot!(plot_result[1], x_true[4,:], x_true[5,:], x_true[6,:],
      label="True", linewidth=2, subplot=1)
scatter!(plot_result[1], x_est[:, 4], x_est[:, 5], x_est[:, 6],
         label="Estimated", markersize=2, subplot=1)
plot!(plot_result[1], xlabel="X [m]", ylabel="Y [m]", zlabel="Z [m]",
      title="3D Trajectory", subplot=1)

# Plot 2: Inverse mass θ = 1/m
m_true_traj = [t < 10.0 ? 1.0 - 0.015*t : 0.85 for t in times]
θ_true_traj = 1 ./ m_true_traj
plot!(plot_result[2], times, θ_true_traj,
      label="True θ=1/m", linewidth=2, subplot=2)
plot!(plot_result[2], times, x_est[:, 7],
      label="Estimated θ", linewidth=2, subplot=2)
plot!(plot_result[2], xlabel="Time [s]", ylabel="θ [kg⁻¹]",
      title="Inverse Mass Parameter", subplot=2)

# Plot 3: Scaled drag φ = Cd/m
Cd_true_traj = [t < 10.0 ? 0.01 : 0.015 for t in times]
φ_true_traj = Cd_true_traj ./ m_true_traj
plot!(plot_result[3], times, φ_true_traj,
      label="True φ=Cd/m", linewidth=2, subplot=3)
plot!(plot_result[3], times, x_est[:, 8],
      label="Estimated φ", linewidth=2, subplot=3)
plot!(plot_result[3], xlabel="Time [s]", ylabel="φ [kg⁻¹]",
      title="Scaled Drag Parameter", subplot=3)

# Plot 4: Position errors
pos_error = sqrt.(sum((x_est[:, 4:6]' .- x_true[4:6,:]).^2, dims=1))[:]
plot!(plot_result[4], times, pos_error,
      label="Position RMSE", linewidth=2, subplot=4)
plot!(plot_result[4], xlabel="Time [s]", ylabel="Error [m]",
      title="Position Estimation Error", subplot=4)

# Plot 5: Velocity errors
vel_error = sqrt.(sum((x_est[:, 1:3]' .- x_true[1:3,:]).^2, dims=1))[:]
plot!(plot_result[5], times, vel_error,
      label="Velocity RMSE", linewidth=2, subplot=5)
plot!(plot_result[5], xlabel="Time [s]", ylabel="Error [m/s]",
      title="Velocity Estimation Error", subplot=5)

plot_result

Key Takeaways

1. Reparameterization is beneficial: By using $\theta = 1/m$ and $\varphi = C_d/m$, we expose a conditionally linear structure that wouldn't be apparent with the original time-varying parameters.

2. Automatic partitioning: The conditionally_linear function correctly identifies that velocities are nonlinear while positions and reparameterized parameters are linear.

3. Time-varying parameters: MUKF successfully tracks time-varying parameters through the random-walk process model.

4. Discretization matters: We use Forward Euler discretization which preserves the conditionally linear structure. Higher-order methods like RK4 would couple the linear and nonlinear substates.

For more details on MUKF theory and implementation, see the LowLevelParticleFilters.jl documentation.

Limitations

• Discretization: The conditionally linear structure may only hold in continuous time. Forward Euler discretization preserves it, but higher-order methods (RK4) typically don't (unless the nonlinear and linear substates are decoupled, such as when estimating sensor bias).
• Approximation: The greedy algorithm is heuristic and may not find the optimal partitioning

Index

• DyadControlSystems.conditionally_linear

result = conditionally_linear(rhs, x; preference = nothing, preference_first = false)

(f_result, g_result) = conditionally_linear(rhs, x, y; Ts = nothing, kwargs...)

result = conditionally_linear(model::System, inputs, y)

using ModelingToolkit
using DyadControlSystems

t, D = ModelingToolkit.t_nounits, ModelingToolkit.D_nounits

@mtkmodel MySystem begin
    @parameters begin
        k = 1.0
    end
    @variables begin
        x1(t) = 0.0
        x2(t) = 0.0
        x3(t) = 0.0
        u(t)
        y(t)
    end
    @equations begin
        D(x1) ~ -x1
        D(x2) ~ 2*x1*x2 + sin(x2)
        D(x3) ~ 2*x2
        y ~ exp(2*x2) * u + k * x1
    end
end

@named model = MySystem()
model = complete(model)

# Partition the system
result = conditionally_linear(model, [model.u], [model.y])

# Access results
println("Nonlinear substate: ", result.f_result.xn)
println("Linear substate: ", result.f_result.xl)