$$ \huge \begin{aligned} \hat{\boldsymbol{\mu}}_{x,k+1} &= \check{\boldsymbol{\mu}}_{x,k+1} + \mathbf{K}_{k+1} \mathbf{z}_{k+1} \\ \hat{\boldsymbol{\Sigma}}_{xx,k+1} &= ( \mathbf{I} - \mathbf{K}_{k+1} \mathbf{C}_{k+1} ) \check{\boldsymbol{\Sigma}}_{xx,k+1} \end{aligned} $$

---

Drone state estimators @ LSY. Contains model free and model based state estimators for drones.

Overview

The estimators are written against the Python Array API standard, so they run with NumPy as well as JAX. They can be batched over many drones, and with JAX also jit compiled.

The documentation covers installation, usage, the ROS 2 node, and the API reference.

Installation

pip install drone-estimators

The ROS 2 node additionally needs a ROS 2 installation. The development setup below provides ROS 2 Kilted through pixi.

Development

git clone https://github.com/learnsyslab/drone-estimators.git
cd drone-estimators
pixi shell

The default environment contains ROS 2 Kilted and installs drone_estimators in editable mode. On first activation, it also clones and builds motion_capture_tracking in ros_ws/.

Run the tests with:

pixi run -e tests tests

Usage

Estimators

import numpy as np

from drone_estimators.estimator_kalman import KalmanFilter

kf = KalmanFilter(dt=1 / 200, model="so_rpy", config="cf2x_L250")
kf.set_state(pos=np.zeros(3), quat=np.array([0.0, 0.0, 0.0, 1.0]))

# Command as roll, pitch, yaw [rad] and collective thrust [N]
kf.predict(dt=1 / 200, command=np.array([0.0, 0.0, 0.0, 0.3]))
estimate = kf.correct(pos=np.array([0.0, 0.0, 0.001]), quat=np.array([0.0, 0.0, 0.0, 1.0]))
print(estimate.pos, estimate.vel, estimate.ang_vel)

To estimate many drones at once, pass a batch_shape:

kf = KalmanFilter(dt=1 / 200, model="so_rpy", config="cf2x_L250", batch_shape=(10,))

For faster estimation loops, jit compile the filter with JAX:

kf = KalmanFilter(dt=1 / 200, model="so_rpy", config="cf2x_L250", batch_shape=(10,), jit=True)

The differentiation-based filter is DiffFilter in drone_estimators.estimator_diff.

ROS 2 node

python -m drone_estimators.ros.node --drone_name cf1 --estimator_type ukf
python -m drone_estimators.ros.node --drone_name cf1 --estimator_type diff
python -m drone_estimators.ros.node --settings path/to/estimators.toml

The settings are read from estimators.toml. To estimate several drones, add one [estimatorX] section per drone and pass your file with --settings. Drones with identical settings are batched into one estimator.

The node subscribes to /tf (e.g., from motion_capture_tracking) and to /drones/<name>/command (roll, pitch, yaw in deg and thrust in PWM, needed by the UKF). It publishes the estimates to /drones/<name>/estimate/{pose,twist,wrench,rotor_vel}. The node only communicates with nodes on the same machine (ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST).

To read the estimates in Python without blocking, use the ROSConnector:

import rclpy

from drone_estimators.ros import ROSConnector

rclpy.init()
conn = ROSConnector(estimator_names=["cf1"], timeout=10.0)
print(conn.est_pos["cf1"], conn.est_vel["cf1"])
conn.close()