$\newcommand{\p}{\boldsymbol{p}} \newcommand{\R}{\boldsymbol{R}} \renewcommand{\v}{\boldsymbol{v}} \renewcommand{\w}{\boldsymbol{w}} \renewcommand{\e}{\boldsymbol{e}} \renewcommand{\q}{\boldsymbol{q}} \newcommand{\calW}{\mathcal{W}} \newcommand{\calA}{\mathcal{A}} \newcommand{\calB}{\mathcal{B}} \newcommand{\bbeta}{\boldsymbol{\beta}} $
I searched on the internet but I couldn't find anything that matches my problem. I am sorry if this question has already been asked. I tried to make the question as clear as possible.
I have a system of different agents moving in 3D space. The configuration of each agent is defined with a pair $\q_i=\left(\p_i, \R_i\right)$ where:
- $\p_i$ is the position of the agent in the world frame
- $\R_i$ is a rotation matrix which describe the attitude of the agent (3 angles)
Let's say that I have 3 agents so the state vector of the system will be $\left(\p_1, \R_1, \p_2, \R_2, \p_3, \R_3,\right)$.
For the $i-th$ agent it holds: \begin{equation}\label{eq:dynamics} \begin{split} \left( \begin{array}{c} \dot\p_i \\ \dot \R_i \end{array} \right)= \left( \begin{array}{cc} \R_i \v \\ \R_i \hat{\w} \end{array} \right) = \\ \left( \begin{array}{cc} \sum_{k}\R_i \e_k \\ 0 \end{array} \right) v_{i_k} + \left( \begin{array}{cc} 0 \\ \sum_{k}\R_i\hat{\e}_k \end{array} \right) w_{i_k} \end{split} \end{equation}
where
$\v_i$ : linear velocity of the $i$~th agent in the world-frame, which is made of the components $v_{i_x},v_{i_i},v_{i_z}$. ($\in \mathbb{R}^3$)
$\w_i$ : angular velocity of the $i$~th agent in the world-frame, which is made of the components $w_{i_x},w_{i_i},w_{i_z}$. ($\in \mathbb{R}^3$)
$\e_k$ : is one of the vectors of the standard basis of $\mathbb{R}^3$ and therefore $\e_k$ selects the coordinate $x,y,z$. The standard basis of $\mathbb{R}^3$ vectors are: \begin{equation}\label{eq:e_vectors}%\small \e_1= \left( \begin{array}{c} 1\\0\\0 \end{array} \right) \;\; , \e_2= \left( \begin{array}{c} 0\\1\\0 \end{array} \right) , \e_3= \left( \begin{array}{c} 0\\0\\1 \end{array} \right) \end{equation}
To be more clear, $\R_i$ allows to rotate a vector from body-coordinates to world-coordinates: \begin{equation} ^{\calW}\v_i=\R_i (^\calB\v_i). \end{equation}
THE QUESTION:
The output of my system are bearing measurements:
\begin{equation} \bbeta_{ij}=\R^T_i\dfrac{\p_j-\p_i}{\|\p_j-\p_i\|}=\R^T_i\dfrac{\p_{ij}}{d_{ij}}\in\mathbb{S}^2, \end{equation}
I want to estimate (observe) the $(\p_i,\R_i)$ of the agents of the formation and I would like to use an EKF (don't worry about the fact whether is possible or not to have a convergent EKF).
I have studied and refreshed my knowledge about the Kalman Filter and EKF but I would like to know how to apply these concepts to the case of a multiagent system and with a representation of the attitude which is in $SO(3)$.
I am dealing with a nonlinear system, so the first thing I will probably need to do is to linearize the system around some equilibrium points at every time step.
Could you point me in the right direction on how to do that?