CoFi — The Constitutional Filter

Resin reasoning · ProMis mission design · CoFi tracking · CoCo control

The Constitutional Filter: Bayesian Estimation of Compliant Agents
Simon Kohaut, Felix Divo, Benedict Flade, Devendra Singh Dhami, Julian Eggert, Kristian Kersting
Paper · IROS 2025 · Citation

CoFi tracks agents that are expected to follow human-interpretable rules, their constitution. The constitution is a Resin program (see its language reference) that ProMis evaluates over a map and facts about the agent, yielding the probability of compliance at every location. CoFi folds this probability into a particle filter, weighted by how much the agent is trusted to comply, so the estimate stays where the agent is allowed to be, and falls back to a plain particle filter when the trust is zero.

Installation

pip install python-cofi

To install from source:

git clone https://github.com/simon-kohaut/CoFi
cd CoFi
pip install -e .

This also installs ProMis, which computes the landscapes of constitutions that CoFi tracks with. Please refer to its usage guide for a reference on the environment representation and reasing aspects.

Quick Start

1. Compute where the agent is allowed to be

import time
import numpy as np
from promis import ProMis, StaRMap
from promis.geo import CartesianMap, CartesianPolygon, CartesianRasterBand, PolarLocation

# The constitution: stay off the obstacle
constitution = """
over(obstacle) <- source("/star_map/over/obstacle", Probability).
landscape if not over(obstacle).
landscape -> target("/landscape").
"""

# A 2 m x 1 m obstacle in a 10 m x 10 m area, whose position is known up to 10 cm
obstacle = CartesianPolygon.from_numpy(np.array([[-1, 1, 1, -1], [-0.5, -0.5, 0.5, 0.5]]), location_type="obstacle")
area = CartesianMap(PolarLocation(0, 0), {"obstacle": [obstacle]})
area.apply_covariance({"obstacle": 0.1**2 * np.eye(2)})

# The probability of complying with the constitution across the area
points = CartesianRasterBand(PolarLocation(0, 0), (50, 50), 10, 10)
star_map = StaRMap(area)
star_map.initialize(points, number_of_random_maps=25, logic=constitution)
promis = ProMis(star_map, constitution, 50 * 50)
promis.initialize(points)
time.sleep(0.05)  # Resin's writers deliver values asynchronously

The constitution has to publish the probability of compliance on the /landscape topic, as in landscape -> target("/landscape").: this is the landscape ProMis hands to CoFi.

2. Track the agent

from cofi import ConstitutionalFilter

# An agent passing above the obstacle at 1 m/s, observed every second with 20 cm of noise
timestamps = np.arange(10.0)
positions = np.stack([timestamps - 4.5, np.full(10, 1.0)]).T + np.random.default_rng(0).normal(0, 0.2, (10, 2))
positions[4:7] = np.nan  # no observations for three seconds

cofi = ConstitutionalFilter(
    promis, trust=0.9,
    sensor_std=0.2, process_noise=0.5, initial_position_std=0.2, initial_velocity_std=1.0,
)
result = cofi.run(positions, timestamps, seed=0)

result.estimates       # (T, 4): east, north, v_east, v_north
result.position_error  # (T,): distance to the observed positions, NaN where there is none

With trust=0.0, ConstitutionalFilter is a plain particle filter. Rows of positions that are NaN mark missing observations: the particles are then only predicted and weighted by the constitution, which keeps the estimate where the agent is allowed to be until it is observed again. sensor_std is the observation noise per second since the last observation, and process_noise the change of velocity per second, both in the units of the positions. With run(..., accelerations=...), measured accelerations, e.g., from an IMU, move the particles' velocities between observations, and process_noise is their noise. When the filter steps more often than the agent is observed, constitution_interval sets the seconds over which the constitution weighs in once, so that its influence does not depend on the step rate.

3. Track the agent online

When observations arrive one at a time, e.g., while ProMis updates the landscape, step the filter online instead:

cofi.start(positions[0], timestamps[0], seed=0)
for position, timestamp in zip(positions, timestamps):
    estimate = cofi.step(position, timestamp)  # east, north, v_east, v_north

Usage Example

We provide a usage example, examples/drones.py, that tracks drones flying through the obstacle course below with a known set of rules. The flights were recorded with CoCo and are included in examples/data.

The drones follow this Resin program, with distances in meters:

over(red) <- source("/star_map/over/red", Probability).
over(green) <- source("/star_map/over/green", Probability).
distance(yellow) <- source("/star_map/distance/yellow", Density).
velocity <- source("/velocity", Number).

rule_1 if not over(red).
rule_2 if distance(yellow) > 0.05.
rule_3 if not over(green).
rule_3 if over(green) and velocity < 0.8.

landscape if rule_1 and rule_2 and rule_3.
landscape -> target("/landscape").

To render the animation above, simply run:

pip install -e ".[examples]"
python examples/drones.py docs/drones.gif

The extra dependencies include ffmpeg on Windows, macOS and Linux; on other platforms, install ffmpeg separately.

Citation

If you use CoFi, please cite the paper below.

@inproceedings{kohaut2025cofi,
  title     = {The Constitutional Filter: Bayesian Estimation of Compliant Agents},
  author    = {Kohaut, Simon and Divo, Felix and Flade, Benedict and Dhami, Devendra Singh
               and Eggert, Julian and Kersting, Kristian},
  booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent
               Robots and Systems (IROS)},
  year      = {2025},
  doi       = {10.1109/IROS60139.2025.11246215}
}