Class FishSchool

Inheritance Relationships

Base Type

  • public IAlgorithm

Class Documentation

class mimir::algorithm::FishSchool : public IAlgorithm

Kinematic fish school simulation model.

This class implements a simple kinematic fish school, which can be controlled by its inputs. The fish school is modelled as a 5 degrees of freedom kinematic body. It has a three dimensional position represented in an inertial reference frame, {NED}, with axes pointing North, East, and Down, respectively. The body can rotate about its y-axis – pitch: \(\theta\), and z-axis – yaw: \(\psi\). The fish school is modeled as an under-actuated entity, meaning that it can only change forward speed (surge) and its angular velocities. The input to the simulation model are surge \(u\), rate of turn \(r\), and desired depth \(D_d\). The desired depth is achieved with a first order response using pitch angular velocity as manipulated variable. The commanded surge and rate of turn is immidiate, i.e. there is no dynamic response. Let us define some quantities to describe the fish school with an ordinary differential equation.

\begin{align} \boldsymbol{p} = \begin{bmatrix}N\\E\\D\\\end{bmatrix} &\in \mathbb{R}^3 &\text{position}\\ \boldsymbol{\Theta} = \begin{bmatrix}\theta\\\psi\end{bmatrix} &\in \mathbb{S}^2 &\text{attitude}\\ \boldsymbol{v} = \begin{bmatrix}u\\v\\w\end{bmatrix} &\in \mathbb{R}^3 &\text{linear velocity}\\ \boldsymbol{w} = \begin{bmatrix}p\\q\\r\\\end{bmatrix} &\in \mathbb{R}^3 &\text{angular velocity}\\ D_d &\in \mathbb{R} &\text{desired depth} \\ \boldsymbol{\eta}(t) = \begin{bmatrix}\boldsymbol{p}\\\Theta\end{bmatrix}, &\quad\mathbb{R} \to \mathbb{R}^3 \times \mathbb{S}^2&\text{state vector} \end{align}

Let \(R_y(\theta), R_z(\psi) \in SO(3)\) be rotation matrixes, so that \(R(\boldsymbol{\eta})\) is the chained rotation defined as follows:

\begin{align*} R_y(\theta) &= \left[\begin{array}{ccc}\cos(\theta)&0 &\sin(\theta)\\0&1&0\\-\sin(\theta)&0&\cos(\theta) \end{array} \right] \\ R_z(\psi) &= \left[\begin{array}{ccc}\cos(\psi)&-\sin(\psi)&0\\\sin(\psi)&\cos(\psi)&0\\0&0&1 \end{array} \right]\\ R(\boldsymbol{\eta}) &= R_z(\psi)R_y(\theta) \\ \end{align*}

We then use the angular velocity transformation \(T(\theta)\) as defined in [10] and the combined state vector transformation \(J(\boldsymbol{\eta})\) for our 5-dimensional system becomes:

\begin{align*} T(\theta) &= \left[\begin{array}{cc}1&0\\0& \frac{1}{\cos(\theta)}\end{array} \right] \\ J(\boldsymbol{\eta}) &= \left[\begin{array}{cc}R(\boldsymbol{\eta})&0\\0& T(\theta)\end{array} \right] \\ \end{align*}

The depth response has a proportional feedback on depth error and a stabilizing term to level out the pitch: \(q_d = 0.1(D - D_d)\cos(2\theta) - \theta\). The resulting ordinary differential equation and input vector are, respectively,

\begin{align*} \dat \eta(t) &= J(\boldsymbol{\eta}) \begin{bmatrix}v\\q_d\\q\end{bmatrix} \\ u(t_d) &= \begin{bmatrix}v_d\\r_d\\Z_d\end{bmatrix}, \end{align*}

where \(t_d\) indicates discrete time points indicated by time_step_ms in the YAML config.

The table below describes the inputs, outputs and parameters of the algorithm. These variables are specified using the YAML configuration file defined further below.










Desired: Surge, rate of turn, depth



[m/s, rad/s, m]








Initial; North, East, Down



\([0, 100, 50]\)



\([\theta_0, \psi_0]\)

Initial; Pitch, Yaw






This numerical model is DEPRECATED in favor of a more detailed model with stochastic behavior defined using FMI [33]. Nevertheless, it may serve as an example on how to implement a simple mimir::IAlgorithm.

Public Functions

explicit FishSchool(const YAML::Node &config, boost::statechart::fifo_scheduler<> &scheduler, boost::statechart::fifo_scheduler<>::processor_handle machine, dds::pub::Publisher publisher, dds::sub::Subscriber subscriber)

FishSchool constructor.

The constructor parses the specification from the given YAML node. It establishes data structures and sets up DDS readers and writers according to the input/output scheme of the algorithm. The following YAML code block shows the expected layout of the FishSchool map of a input config file. The inputs, outputs, and initial conditions are communicated with DDS communication. Common is their DDS topic and DDS identifier. The specification is deemed self-explanatory.

time_step_ms: 200
  fish_ctrl:                # DDS type: fkin::IdVec3d
    topic: fkinFishCtrl
    id: Fish
    max_age_ms: -1
outputs:                    # DDS type: fkin::Kinematics6D
    topic: fkinKinematics6D
    id: Fish
  position:                 # DDS type: fkin::IdVec3d
    topic: fkinPosition
    id: FishInit
    max_wait_ms: 80
    fallback: [0, 100, 50]
  euler:                    # DDS type: fkin::IdVec3d
    topic: fkinEuler
    id: FishInit
    max_wait_ms: 80
    fallback: [0, 0, 0]

  • config[in] YAML configuration from input file.

  • scheduler[in] State machine scheduler, needed to post events to state machine.

  • machine[in] State machine processor handle, needed to post events to state machine.

  • publisher – DDS data writer to send data.

  • subscriber – DDS data reader to receive data.

virtual ~FishSchool()


virtual void solve(const std::atomic<bool> &cancel_token)

See base class for details.

The algorithm uses CVODES from SUNDIALS [17].

virtual void initialize(const std::atomic<bool> &cancel_token)

See base class.

virtual void timer(const std::atomic<bool> &cancel_token)

See base class.

inline virtual const char *name()

Name identifier of algorithm.

void event(boost::statechart::event_base *const event)

Helper function that queues event to state machine.

Private Functions

inline void step_time()

March simulation time one time step ahead.

FishSchool() = delete

Private Members

std::unique_ptr<Impl> m_impl

Holds the implementation of the algorithm.

boost::statechart::fifo_scheduler &m_scheduler

Members for state machine.

boost::statechart::fifo_scheduler::processor_handle m_stateMachine

Member for state machine.

const std::chrono::milliseconds m_time_step

Discrete time step.

std::chrono::steady_clock::time_point m_next_step

Simulation time point.

const YAML::Node m_config

YAML configuration for algorithm.