State Estimation

In this tutorial, you'll write a BB.Estimator from scratch — a small sensor-nested filter that consumes raw accelerometer data and republishes it with a fused tilt estimate. By the end you'll understand the behaviour, the DSL entity, and how messages flow through an estimator.

Prerequisites

Complete Sensors and PubSub. You should know how a sensor publishes BB.Message.Sensor.Imu (or similar) into pubsub and how to subscribe.

The estimator's job

An estimator consumes one or more input message streams and publishes derived state. The same contract covers two distinct cases:

• Within-sensor fusion — combining the modalities of a single sensor. AHRS combining gyro + accelerometer from one IMU into orientation is the canonical case.
• Cross-sensor fusion — combining different physical sensors into an estimate of some target frame's state. An EKF blending IMU + wheel odometry into a base pose is the canonical case.

This tutorial focuses on the within-sensor form because it's the simplest place to start. The cross-sensor form is covered in the understanding-estimators topic.

The example we'll build

A TiltSmoother that nests inside an IMU sensor, consumes its Imu messages, and republishes them with the :orientation field set to a low-pass tilt estimate computed from the accelerometer. (A real AHRS would also use the gyro — bb_estimator_ahrs does — but a single-axis low-pass keeps the example small.)

Step 1: Declare the estimator in your DSL

estimator is a DSL entity that nests inside either a sensor (sensor-nested form — what we want) or a link (cross-sensor form — for later).

defmodule MyRobot.Robot do
  use BB

  topology do
    link :base_link do
      sensor :imu, MyImuSensor, bus: "i2c-1", address: 0x68 do
        estimator :tilt, {TiltSmoother, alpha: 0.95}
      end
    end
  end
end

The estimator declaration takes the same shape as sensor and actuator — a name (:tilt) and a child spec ({TiltSmoother, alpha: 0.95}).

Because :tilt is nested inside sensor :imu, the framework wires the parent sensor's published messages as its implicit input. No input blocks are needed.

Step 2: Write the callback module

Like sensors and controllers, your module is not a GenServer — the framework provides a wrapper (BB.Estimator.Server) that delegates to your callbacks. You implement BB.Estimator.

defmodule TiltSmoother do
  use BB.Estimator,
    options_schema: [
      alpha: [type: :float, default: 0.95, doc: "Low-pass weight"]
    ]

  alias BB.Math.{Quaternion, Vec3}
  alias BB.Message
  alias BB.Message.Sensor.Imu

  @impl BB.Estimator
  def init(opts) do
    {:ok, %{alpha: Keyword.fetch!(opts, :alpha), q: Quaternion.identity()}}
  end

  @impl BB.Estimator
  def handle_input(%Message{payload: %Imu{} = imu} = msg, state) do
    {ax, ay, az} = {Vec3.x(imu.linear_acceleration),
                    Vec3.y(imu.linear_acceleration),
                    Vec3.z(imu.linear_acceleration)}

    # Tilt from gravity (roll about X, pitch about Y), ignoring yaw.
    roll = :math.atan2(ay, az)
    pitch = :math.atan2(-ax, :math.sqrt(ay * ay + az * az))

    q_accel = Quaternion.from_euler(roll, pitch, 0.0)
    q_blended = Quaternion.slerp(state.q, q_accel, 1.0 - state.alpha)

    {:ok, out} =
      Imu.new(msg.frame_id,
        orientation: q_blended,
        angular_velocity: imu.angular_velocity,
        linear_acceleration: imu.linear_acceleration
      )

    {:reply, [out: out], %{state | q: q_blended}}
  end

  def handle_input(_other, state), do: {:noreply, state}
end

Two things are worth pausing on.

The reply shape

Sensors and actuators publish by calling BB.PubSub.publish/3 directly. Estimators don't — they return outputs from their callback:

{:reply, [out: message], new_state}

The framework routes each {output_name, message} tuple to that output's pubsub path. :out is the conventional single-output name and the framework synthesises a path for it automatically — your estimator's own path, in this case [:sensor, :base_link, :imu, :tilt]. Multi-output estimators declare each output explicitly with an output :name block; this tutorial doesn't need them.

You can return {:noreply, state} to consume an input without emitting anything — useful for accumulators that need several updates before producing one output.

The input shape

For sensor-nested estimators (or link-nested estimators with one declared input), handle_input/2 receives a single %BB.Message{} envelope. Multi-input estimators receive a map %{input_name => message} keyed by their declared input names. The same callback handles both — pattern-match on whichever shape your estimator expects.

For Roboticists: This is similar in spirit to a ROS node that subscribes to /imu/data_raw and republishes on /imu/data. The difference is that BB's framework owns subscription, fan-in, dt tracking, and output routing — your module is pure logic.

For Elixirists: Think of BB.Estimator as a behaviour that gives you a GenServer with structured input/output semantics — handle_input/2 is your message handler, the {:reply, outputs, state} return shape is the publish side. You can also handle arbitrary messages via handle_info/2, handle_call/3, handle_cast/2, and any of them can return the {:reply, outputs, state} shape too — handy for estimators that emit on a timer.

Step 3: Subscribe to the estimator's output

The estimator publishes to its natural path. For sensor-nested estimators that's the parent sensor's path with the estimator name appended:

{:ok, _} = BB.subscribe(MyRobot, [:sensor, :base_link, :imu, :tilt])

# ... in handle_info ...
def handle_info({:bb, _path, %BB.Message{payload: %BB.Message.Sensor.Imu{} = imu}}, state) do
  {roll, pitch, _yaw} = BB.Math.Quaternion.to_euler(imu.orientation)
  IO.puts("roll=#{Float.round(roll, 3)} pitch=#{Float.round(pitch, 3)}")
  {:noreply, state}
end

Subscribers to [:sensor, :base_link, :imu] still receive the raw sensor output — the estimator publishes alongside, not in place of, the parent sensor.

For Roboticists: The two are siblings in pubsub-space. If a downstream consumer wants raw IMU data and another wants the fused output, both subscribe to the paths they care about.

Step 4: Test the estimator

Algorithms are easier to test as pure functions. Expose a stateless step/N helper so tests can drive it without spinning up a GenServer:

defmodule TiltSmoother do
  # ... as before ...

  @doc "Run one step against an `{ax, ay, az}` accel tuple."
  def step(state, {ax, ay, az}) do
    # same body, just refactored out of handle_input
  end
end

Then in your tests:

defmodule TiltSmootherTest do
  use ExUnit.Case, async: true

  test "stationary gravity along +Z stays near identity" do
    state = %{alpha: 0.9, q: BB.Math.Quaternion.identity()}

    final =
      Enum.reduce(1..100, state, fn _, s ->
        TiltSmoother.step(s, {0.0, 0.0, 9.81})
      end)

    assert_in_delta BB.Math.Quaternion.angular_distance(
                      final.q,
                      BB.Math.Quaternion.identity()
                    ),
                    0.0,
                    1.0e-3
  end
end

For integration tests, spin up the whole robot:

test "estimator publishes when sensor input arrives" do
  start_supervised!(MyRobot.Robot)
  {:ok, _} = BB.subscribe(MyRobot.Robot, [:sensor, :base_link, :imu, :tilt])

  # Publish a fake IMU message at the parent sensor's path:
  {:ok, msg} = BB.Message.Sensor.Imu.new(:imu,
    orientation: BB.Math.Quaternion.identity(),
    angular_velocity: BB.Math.Vec3.zero(),
    linear_acceleration: BB.Math.Vec3.new(0.0, 0.0, 9.81)
  )

  BB.publish(MyRobot.Robot, [:sensor, :base_link, :imu], msg)

  assert_receive {:bb, [:sensor, :base_link, :imu, :tilt],
                  %BB.Message{payload: %BB.Message.Sensor.Imu{}}}, 500
end

Common gotchas

Path conventions

• Sensor-nested estimators publish on [:sensor | sensor_path] ++ [estimator_name]. For the example above that's [:sensor, :base_link, :imu, :tilt].

• Link-nested estimators publish on [:estimator | link_path] ++ [estimator_name] — the :estimator prefix distinguishes them from sensor outputs in subscriptions.

When the verifier rejects an estimator with "input :foo references unknown path …" it usually means a typo in the path or that the referenced sensor lives at a different level of the topology than you think.

dt tracking

Each incoming message carries monotonic_time in nanoseconds. An estimator that needs dt should store the previous message's monotonic_time and compute the delta on each handle_input/2. The framework doesn't compute dt for you — that decision is per-algorithm (some don't need it, some prefer a fixed-rate approximation).

Unit conventions

BB uses SI everywhere: rad/s for angular velocity, m/s² for linear acceleration, radians for orientation, metres for translation. Algorithms that traditionally work in other units (e.g. accel_threshold expressed as "fraction of 1 g") should convert at the boundary. Don't push unit awareness into sensor drivers — they should publish SI.

Next steps

• The understanding-estimators topic explains the design choices behind the behaviour and DSL — single vs multi-input, frame semantics, why the reply shape, where estimators sit in the supervision tree.
• The configure estimator health how-to covers latency_budget, lost_after, and the on_degraded/on_lost/on_recovered command-as-policy mechanism.
• The bb_estimator_ahrs sibling package is a real-world example: three IMU fusion algorithms (Madgwick, Mahony, Complementary), each implemented as a BB.Estimator. Skim the source for patterns to copy.