From an install to a closed loop

Install

The library is one module with five packages. Add it to a project with moon add, which writes the dependency into moon.mod.

moon add moonbitstack/moonmcl

Import the root package under an alias to reach the whole surface. The examples use mctl; any name works. If you want only one package, import it directly — "moonbitstack/moonmcl/control", for instance — and the other four are not compiled.

import {
  "moonbitstack/moonmcl" @mctl,
}

Every quantity is a Double. Angles are radians, lengths are metres, and time is seconds. The controllers hold their state inside the value you own, so nothing is global and two controllers never interfere.

A first loop

A PID controller takes an error and a timestep and returns the output. Here it drives a first-order plant from rest towards a setpoint of one, with the output clamped to ±10.

let pid = @mctl.Pid::new(config=@mctl.pid_gains(kp=2.0, ki=0.5, kd=0.1))
let mut y = 0.0
for _ in 0..<100 {
  let u = pid.command(reference=1.0, measurement=y, dt=0.02)
  y = y + 0.02 * (u - y)
}
println(y.to_string())

The controller is stateful: call update once per tick and it accumulates the integral and remembers the previous error. A plant that reaches only 0.78 after two seconds has not settled — raise kp, or leave the loop running longer.

Tuning the loop

Three refinements matter in practice, and all three are built in.

Anti-windup. While the output sits at its limit and the error would push it further out, the integrator is held. A long call at a target the plant cannot reach therefore leaves nothing behind to overshoot with once the target comes back into range.

Derivative on the measurement. update differentiates the error, which is what the textbook formula says. A step in the setpoint then produces an infinite derivative and a spike in the output. update_signals(setpoint, measurement, dt) differentiates the measurement instead, so the same step produces nothing:

let config = @mctl.pid_gains(kp=0.0, ki=1.0)
let pid = @mctl.Pid::new(config={ ..config, output: { low: -0.5, high: 0.5 } })
for _ in 0..<100 {
  ignore(pid.command(reference=1.0, measurement=0.0, dt=0.1))
}
// The output saturates at half a unit and the integrator comes to rest there.
println(pid.integral().to_string())

Feedforward. A term added to every output, for the part of the command that is known in advance. The compensator takes a gain on the reference and a gain on its rate.

let profile = @mctl.trapezoid(distance=2.0, v_max=1.0, a_max=1.0)
let sample = profile.sample(profile.duration() / 2.0)
let feed = @mctl.Feedforward::new(velocity=0.8, acceleration=0.2)
println(feed.command(sample).to_string())

Trajectories

A jerk-limited profile is the shortest rest-to-rest move that respects a peak velocity, a peak acceleration and a peak jerk. scurve returns the profile; at gives the four kinematic quantities at any instant.

let profile = @mctl.scurve(distance=100.0, v_max=5.0, a_max=2.0, j_max=1.0)
let sample = profile.sample(10.0)
println(
  profile.duration().to_string() +
  " s, then at t = 10 s: " + sample.position.to_string() +
  " m at " + sample.velocity.to_string() + " m/s",
)

A hundred metres under those limits takes 24.5 seconds. At ten seconds the move has reached its cruise: five metres per second, no acceleration. Sampling at over the whole duration is how the showcase page draws the curve.

A move can also begin where the previous one left off, with the acceleration already non-zero. Both boundary accelerations are folded into the first and last ramp, so the profile carries the acceleration it was given instead of spending time taking it to zero and building it up again.

let profile = @mctl.scurve(
  distance=3.0,
  v_max=2.0,
  a_max=1.5,
  j_max=6.0,
  start_velocity=0.8,
  start_acceleration=0.4,
  end_velocity=1.2,
  end_acceleration=0.6,
)
println(((profile.duration() * 1000.0).round() / 1000.0).to_string())

Under those limits the move takes 1.989 seconds, starting at 0.4 and ending at 0.6 m/s² of acceleration. It is the shortest such move: the peak the profile ramps to is the one the distance can pay for.

The profile covers motion along one direction, from any boundary state inside the limits. A move that would have to run backwards in the middle to make its distance — three metres per second down to one over half a metre, say — is refused with Unreachable rather than returned with an overshoot: coming down to one takes four metres of braking, and the half metre is not there. Give it room and the same boundary states produce an ordinary profile.

Linear algebra

Vectors and matrices are flat arrays of Double behind a small wrapper. The one operation worth calling out is solve, which factorises with partial pivoting rather than inverting:

let stiff = @mctl.Mat::of([[2.0, 1.0], [1.0, 3.0]])
let load = @mctl.Mat::of([[3.0], [5.0]])
println(@mctl.solve(stiff, load).to_string())

A singular system aborts rather than returning a plausible-looking answer. inv, qr, kron and the products are there for the cases that need them; everything else in the library is built on mat_vec, mat_mat and solve.

Optimal feedback

For x[k+1] = A x[k] + B u[k], with a state weight Q and a control weight R, the discrete Riccati equation gives the gain that minimises the infinite-horizon cost. The library iterates it to a tolerance and returns K.

let a = @mctl.Mat::of([[1.0, 0.1], [0.0, 1.0]])
let b = @mctl.Mat::of([[0.005], [0.1]])
let q = @mctl.Mat::identity(2)
let r = @mctl.Mat::of([[1.0]])
println(@mctl.lqr(a=a, b=b, q=q, r=r).gain().to_string())

The control is u = -K x. Because the gain depends only on the model and the weights, it is computed once and used every tick. When following a non-zero reference instead of regulating to zero, the integral form adds one state and removes the standing error: integral_lqr(a, b, c, q, q_int, r, reference, dt).

Receding horizon

The same plant under a finite-horizon plan. Each call solves a quadratic program over the horizon and returns the first control; the caller applies it, measures again, and calls step afresh.

let a = @mctl.Mat::of([[1.0, 0.1], [0.0, 1.0]])
let b = @mctl.Mat::of([[0.005], [0.1]])
let model = @mctl.StateSpace::discrete(
  a=a, b=b, c=@mctl.Mat::identity(2), d=@mctl.Mat::zeros(2, 1), dt=0.1,
)
let mpc = @mctl.Mpc::new(
  model=model,
  state_weight=@mctl.Mat::identity(2),
  input_weight=@mctl.Mat::of([[1.0]]),
)
println(mpc.control(state=@mctl.Vec::of([1.0, 0.0]), target=@mctl.Vec::zeros(2)).to_string())

The horizon is the number of steps planned ahead; input_limit clamps every element of the plan, which is what makes the result differ from the LQR gain. The step is not cheap — it builds and solves a dense system — so it suits a slower outer loop, with a PID or an LQR closing the fast one.

State estimation

A Kalman filter alternates prediction with the model and correction with a measurement. Twenty corrections against an unchanging measurement of two leave the estimate almost exactly there:

let model = @mctl.StateSpace::discrete(
  a=@mctl.Mat::of([[1.0]]), b=@mctl.Mat::of([[0.0]]),
  c=@mctl.Mat::of([[1.0]]), d=@mctl.Mat::zeros(1, 1), dt=1.0,
)
let kf = @mctl.Kalman::new(
  model=model,
  process=@mctl.Mat::of([[0.01]]),
  measurement=@mctl.Mat::of([[0.1]]),
  state=@mctl.Vec::of([0.0]),
  covariance=@mctl.Mat::of([[1.0]]),
)
for _ in 0..<20 {
  kf.predict(@mctl.Vec::of([0.0]), 1.0)
  kf.correct(@mctl.Vec::of([2.0]))
}
println(kf.state().to_string())

The extended filter takes the two models as closures and differentiates them by central differences, so a nonlinear plant costs no Jacobian algebra. Here a constant-velocity target is tracked from a position measurement:

let plant = @mctl.Dynamics::new(
  f=fn (x, u, dt) {
    ignore(u)
    @mctl.Vec::of([x.get(0) + dt])
  },
  h=fn (x) { @mctl.Vec::of([x.get(0)]) },
)
let ekf = @mctl.ExtendedKalman::new(
  model=plant,
  process=@mctl.Mat::of([[0.01]]),
  measurement=@mctl.Mat::of([[0.1]]),
  state=@mctl.Vec::of([0.0]),
  covariance=@mctl.Mat::of([[1.0]]),
)
for _ in 0..<20 {
  ekf.predict(@mctl.Vec::of([0.0]), 1.0)
  ekf.correct(@mctl.Vec::of([2.0]))
}
println(ekf.state().to_string())

Four seconds in, the filter has recovered the position and a velocity of one metre per second from position measurements alone.

A run of estimates can be refined once the measurements are in. rts takes the filtered means, the filtered covariances and the transitions between them, and walks backwards: every estimate is pulled towards what the step after it implies, so an early pose is corrected by the measurements that came later. The last entry comes back untouched, which is where the backward pass starts, and no covariance grows. The backward pass on its own is replayed against a mature filter library on that library's own record, so a disagreement there is the smoother's and not the filter's.

A plant given in physical units is usually continuous. discretize turns it into the record the filter and the predictive controller want, by zero-order hold — the input held over the period, which is what a digital controller actually does — or by the trapezoidal rule or forward Euler when that is what a plant model was written for. The hold is the matrix exponential of the augmented plant, so it stays exact at any period it is asked for.

Planning

A grid is free or blocked per cell, four-connected, with unit edge cost. index(col, row) turns a coordinate into the flat index the search uses.

let grid = @mctl.Grid::new(10, 10)
for column in 4..<6 {
  grid.block(column, 4)
  grid.block(column, 5)
}
let path = @mctl.grid_path(
  grid,
  start=grid.node(0, 0),
  goal=grid.node(9, 9),
  search=@mctl.GridSearch::AStar,
)
println(path.length().to_string())

Nineteen cells is the Manhattan distance plus one: the detour around the two-by-two block costs nothing extra. grid_path returns None when the goal is walled off, so unwrap is a deliberate choice to abort instead of steering nowhere.

A car's own geometry

A car that can only go forward has six words between two poses: three pieces, each a left arc, a right arc or a straight. A car that can also reverse has forty-eight, because every word can be mirrored, driven backwards, or both. Both models are state spaces, so the distance between two poses is the length of the shortest path and the interpolation walks it.

let car = @mctl.ReedsShepp::new(turning_radius=1.0)
let start = @mctl.Vec::of([0.0, 0.0, 0.0])
let goal = @mctl.Vec::of([-2.0, 0.8, 0.5])
let path = car.path(start~, goal~)
println(
  ((path.length * 1000.0).round() / 1000.0).to_string() +
  " m, pieces: " + path.segments.length().to_string() +
  ", cusps: " + path.cusps().to_string(),
)

The goal is behind the start, so the car backs up once: three pieces and one cusp. The same goal costs a forward-only car 7.56 metres, because every metre of it has to be driven facing the way it is going. Since a car space is a Space, a tree grown in it is made of arcs and reversals rather than straight lines.

Drive kinematics

A differential drive has one track width and two wheels. The inverse map turns a body twist into wheel speeds; the forward map turns wheel speeds back into a twist; Differential::advance integrates the twist over a timestep.

let base = @mctl.Differential::new(wheel_radius=0.1, track=0.5)
let wheels = base.inverse(@mctl.Vec::of([1.0, 0.5]))
let pose = base.advance(
  pose=@mctl.Vec::of([0.0, 0.0, 0.0]),
  twist=@mctl.Vec::of([1.0, 0.0]),
  dt=0.1,
)
println(
  wheels.to_string() + " rad/s, then the pose after 0.1 s: " +
  "[ " + pose.get(0).to_string() + ", " + pose.get(1).to_string() + " ]",
)

At one metre per second and half a radian per second on a half-metre track, the wheels turn at 0.875 and 1.125. Driving straight for a tenth of a second moves the robot a tenth of a metre along x.

Path following

Two geometric laws, both returning a steering angle and a speed. Pure pursuit aims at a point on the path ahead of the robot; Stanley corrects both the heading error and the lateral offset.

let path = [@mctl.Vec::of([0.0, 0.0]), @mctl.Vec::of([10.0, 0.0])]
let steer = @mctl.pursuit.steering(pose=@mctl.Vec::of([0.0, 0.0, 0.2]), path=path)
println(steer.to_string())

A lookahead point 3 m ahead and 0.4 m to the left asks for about five degrees of steering on a one-metre wheelbase. The showcase page runs both laws on the same circle from different starting offsets; pure pursuit settles onto the path, while Stanley — given only heading and offset, with no curvature feedforward — holds a small standing offset on a curve.

One loop, end to end

The pieces compose without glue. A profile supplies a speed, the inverse kinematics split it into wheel speeds, the forward kinematics rebuild the twist, and the pose integrates. A five-metre move on a half-metre track:

let profile = @mctl.scurve(distance=5.0, v_max=1.0, a_max=0.8, j_max=4.0)
let base = @mctl.Differential::new(wheel_radius=0.1, track=0.5)
let mut pose = @mctl.Vec::of([0.0, 0.0, 0.0])
for i in 0..<200 {
  let sample = profile.sample(i.to_double() * 0.05)
  let wheels = base.inverse(@mctl.Vec::of([sample.velocity, 0.0]))
  pose = base.advance(pose=pose, twist=base.forward(wheels), dt=0.05)
}
println(pose.get(0).to_string())

The move takes 6.45 seconds and the robot stops within a rounding error of five metres. Add a PID on the measured distance, an LQR on the drive, or a Kalman filter on the odometry, and the same loop carries the rest of the library unchanged.