%###########################################################################
%
%   State Estimation Framework
%
%###########################################################################
\chapter{State Estimation Framework} \label{chap:estimation}
Filtering methods involve estimating the robot’s current state using the set of all measurements up to the current time. With a linear process and measurement model and white Gaussian noises, the Kalman Filter provides an estimate of the state vector \enquote{$\xb$} along with a corresponding covariance matrix \enquote{$\Pb$}, which specifies the uncertainty of the estimate. The filter involves two steps: prediction and update. In the prediction step, the estimates are propagated through the system dynamics in order to produce a priori estimates and covariance matrix, and in the update step, the estimates are updated using a measurement to produce a posteriori estimates and covariance matrix. However, the standard Kalman Filter is applicable only for state estimation in linear systems. Most practical robots have nonlinear system dynamics and mainly sensory data are also nonlinear; therefore, for these cases an Extended Kalman Filter (EKF) can be employed, which utilizes Taylor series expansions to linearize the process and measurement models around the current state estimate. Although, the convergence and optimality are no longer guaranteed for such a filter, it is mostly advantageous because of its performance and simplicity, and being computationally efficient. In our case the process model is dynamics of the robot’s base, and we use the base linear velocity measured from kinematics of the set of legs in contact with ground as the measurement model. Since our robot has a free floating joint in the base (with 6 DoF), we use quaternion in order to represent the orientation of the base and to avoid singularities in other representations such as Euler-angles.
%
\section{Mathematical Prerequisites} \label{sec:math}
The number of degrees of freedom (DoF) of a robot is the smallest number of real-valued coordinates needed to represent its configuration \cite{Lynch}. In order to specify the position and orientation of a rigid body in three-dimensional physical space, a minimum of six numbers are needed. In this section we give an overview of a systematic way to describe a rigid body’s position and orientation which relies on attaching a reference frame to
the body. The configuration of this frame with respect to a fixed reference frame is then represented as a $4\times4$ matrix. This matrix is an example of an implicit representation of the C-space (configuration space), the actual six-dimensional space of rigid-body configurations is obtained by applying ten constraints to the 16-dimensional space of $4\times4$ real matrices.
%
\subsubsection{Rigid Body Rotation}
In $\mathbb{R}^3$ , the rotation group $SO(3)$ is the group of rotations around the origin under the operation of composition. Rotations are linear transformations that preserve vector length and relative vector orientation. Its importance in robotics is that it represents rotations of rigid bodies in 3D space: a \enquote{rigid motion} requires precisely that distances, angles and relative orientations within a rigid body be preserved upon motion. In contrast, if norms, angles or relative orientations are not kept upon motion, the body could not be considered rigid.
The special orthogonal group $SO(3)$, or the group of rotation matrices, is the set of all $3\times3$ real matrices $\Rb \in \mathbb{R}^{3\times3}$ that satisfy
%
\begin{align} \label{eq:SO(3)}
	\Rb^T\Rb=\Ib \\
	det(\Rb)=1.
\end{align}
%
We can define the rotation operator 
\begin{align}
	r:\mathbb{R}^3\rightarrow\mathbb{R}^3 \nonumber\\
	\vb\mapsto r(\vb)
\end{align}
acting on vector $\vb \in \mathbb{R}^3$ as a linear operator; since it is defined from the scalar and vector products, which are linear. Therefore it can be represented by a matrix $\Rb \in \mathbb{R}^{3\times3}$, which produces rotations to vectors through the matrix product
%
\begin{align} \label{eq:rotation_operator}
	r(\vb) = \Rb\vb.
\end{align}
%
\subsubsection{The Rotation Group and the Quaternion}
In this section, we would like to mention the connection between quaternions and rotation matrices as representations of the rotation group $SO(3)$.
The set of unit quaternions forms a group under the operation of multiplication. This
group is topologically a 3-sphere, that is the 3-dimensional surface of the unit sphere of $\mathbb{R}^4$, and is commonly noted as $S^3$.\par
%
Similar to~\eqref{eq:SO(3)}, the corresponding two properties of quaternions as rotation operators are
%
\begin{align}
	||\qb||^2 &= 1 \nonumber \\
	\qb^{*}\otimes\qb &= \qb\otimes\qb^{*} = 1
\end{align}
%
where $\qb^{*}$ stands for the conjugate of the quaternion.\par
%
The equivalent formula to~\eqref{eq:rotation_operator} can be written as quaternion rotation action
%
\begin{align}
	r(\vb) = \qb\otimes \vb \otimes \qb^{*}.
\end{align}
%
For the sake of completeness, we also mention the corresponding ordinary differential equations (ODE) for rotation matrix and quaternion, which later are needed for the derivation of EKF. For detailed discussion and derivation, one can refer to \cite{Sola}.
%
\begin{align} 
	\dot{\Rb} &= \Rb[\boldsymbol{\omega}]_{\times} \\
	\dot{\qb} &= \frac{1}{2}\qb\otimes \boldsymbol{\omega} = \frac{1}{2} \boldsymbol{\Omega}_R(\boldsymbol{\omega})\qb \label{eq:ode}
\end{align}
%
Where $[.]_\times$ stands for skew-symmetric operator, and $\boldsymbol{\Omega}_R(\boldsymbol{\omega}) \in \mathbb{R}^{4\times4}$ is the right-quaternion-product matrix for pure quaternion $\boldsymbol{\Omega}=[0, {\omega}_x, {\omega}_y, {\omega}_z]^T$
%
\begin{align}
	[\boldsymbol{\omega}]_{\times} = 
	\begin{bmatrix}
		0 & -{\omega}_z & {\omega}_y\\
		{\omega}_z & 0 & -{\omega}_x\\
		-{\omega}_y & {\omega}_x & 0
	\end{bmatrix}
\end{align}
%
\begin{align}
	\boldsymbol{\Omega}_R(\boldsymbol{\omega}) = 
	\begin{bmatrix}
		0 & -\boldsymbol{\omega}^T\\
		\boldsymbol{\omega} & -[\boldsymbol{\omega}]_{\times}
	\end{bmatrix}.
\end{align}
%
\subsubsection{Rigid Body Motion}
We now consider representations for the combined orientation and position of a rigid body. A natural choice would be to use a rotation matrix $\Rb \in SO(3)$ to represent the orientation of the body frame $B$ in the fixed frame $W$ and a vector $\boldsymbol{p} \in \mathbb{R}^3$ to represent the origin of $B$ in $W$. Rather than identifying $\Rb$ and $\boldsymbol{p}$ separately, they are packed into a single matrix as \enquote{The special Euclidean group $SE(3)$}.\par
%
The special Euclidean group SE(3), also known as the group of rigid-body motions or homogeneous transformation matrices in $\mathbb{R}^3$, is the set of all $4\times4$ real matrices $\Tb$ of the form
%
\begin{align}
	{^W}\Tb_{WB} = 
	\begin{bmatrix}
		{^W}\Rb_{WB} & {^W}\boldsymbol{p}_{WB}\\
		0 & 1
	\end{bmatrix}
\end{align}
%
where $\Rb\in SO(3)$, and $\boldsymbol{p} \in \mathbb{R}^3$ are respectively a rotation matrix and a column vector.\par
%
There are three major uses for a transformation matrix $\Tb$:
\begin{itemize}
	\item To represent the configuration (position and orientation) of a rigid body.
	\item To change the reference frame in which a vector or frame is represented.
	\item To displace a vector or frame.
\end{itemize}
%
\section{Kalman Filter} \label{sec:kf}
Kalman filtering \cite{Kalman} is based on linear dynamical systems discretized in the time domain. We consider the linear discrete dynamic system as described in~\eqref{eq:linear_system}, where both prediction $\boldsymbol{F}_k$ and measurement $\boldsymbol{H}_k$ models are corrupted by zero-mean Gaussian process noise $\boldsymbol{w}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{Q}_k)$, and measurement noise $\boldsymbol{v}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{R}_k)$ respectively
%
\begin{align} \label{eq:linear_system}
	\boldsymbol{x}_k &= \boldsymbol{F}_k\boldsymbol{x}_{k-1} + \boldsymbol{B}_k\boldsymbol{u}_{k} + \boldsymbol{w}_k \nonumber \\
	\boldsymbol{y}_k &= \boldsymbol{H}_k\boldsymbol{x}_{k} + \boldsymbol{v}_k.
\end{align}
%
The goal of the Kalman Filter is to estimate the mean $\boldsymbol{\mu}$ and covariance matrix $\boldsymbol{P}$ (which specifies the uncertainty of the estimate) of the Gaussian distribution over the state $\boldsymbol{x}_k$ , given the previous state $\boldsymbol{x}_{k-1}$, the current control input $\boldsymbol{u}_k$, and the current measurement $\boldsymbol{y}_k$.\par
%
The filter involves two steps: propagating the estimates through the system in the prediction step to produce \enquote{\textit{a priori}} estimates $\boldsymbol{\mu}_k^{-}$ and $\boldsymbol{P}_k^{-}$ (indicated by minus superscript), and then updating the estimates using a measurement in the update step to produce \enquote{\textit{a posteriori}} estimates $\boldsymbol{\mu}_k^{+}$ and $\boldsymbol{P}_k^{+}$ (indicated by plus superscript).\par
%
The Kalman filter is a recursive estimator. This means that only the estimated state from the previous time step and the current measurement are needed to compute the estimate for the current state.
%
\subsubsection{Prediction Step}
First we calculate the predicted \enquote{a priori} state estimate and estimate covariance matrix
%
\begin{align} \label{eq:prediction}
	\boldsymbol{\mu}_{k+1}^{-} &= \boldsymbol{F}_k\boldsymbol{\mu}_{k}^{+} + \boldsymbol{B}_k\boldsymbol{u}_{k} \nonumber \\
	\boldsymbol{P}_{k+1}^{-} &= \boldsymbol{F}_k\boldsymbol{P}_k^{+}\boldsymbol{F}_k^T + \boldsymbol{Q}_k.
\end{align}
%
\subsubsection{Update Step}
Then the updated \enquote{a posteriori} state estimate and estimate covariance matrix are computed as follows
\begin{align} \label{eq:update}
	\boldsymbol{K}_k &= \boldsymbol{P}_{k+1}^{-}\boldsymbol{H}_k^T (\boldsymbol{H}_k\boldsymbol{P}_{k+1}^{-}\boldsymbol{H}_k^T + \boldsymbol{R}_k)^{-1} \nonumber \\
	\boldsymbol{P}_{k+1}^{+} &= (\boldsymbol{I} - \boldsymbol{K}_k\boldsymbol{H}_k) \boldsymbol{P}_{k+1}^{-} \nonumber \\
	\boldsymbol{\mu}_{k+1}^{+} &= \boldsymbol{\mu}_{k+1}^{-} + \boldsymbol{K}_k (\boldsymbol{y}_k - \boldsymbol{H}_k\boldsymbol{\mu}_{k+1}^{-})
\end{align}
where $\boldsymbol{K}_{k}$ is the optimal Kalman gain.
%
\section{Extended Kalman Filter} \label{sec:ekf}
Suppose we have the discrete-time nonlinear system as shown in the~\eqref{eq:nonlinear_system}. The state is first predicted using the non-linear discrete transition function $\boldsymbol{f}(.)$ (prediction model) and then corrected by the observation function $\boldsymbol{h}(.)$ (measurement model). The functions are corrupted by zero-mean Gaussian process noise $\boldsymbol{w}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{Q}_k)$ and measurement noise $\boldsymbol{v}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{R}_k)$
%
\begin{align} \label{eq:nonlinear_system}
	\boldsymbol{x}_k &= \boldsymbol{f}(\boldsymbol{x}_{k-1},
	\boldsymbol{u}_{k-1}, \boldsymbol{w}_k) \nonumber\\
	\boldsymbol{y}_k &= \boldsymbol{h}(\boldsymbol{x}_k, \boldsymbol{v}_k).
\end{align}
%
We can simply linearize these models around the current estimate at ($t_k$), in order to find the prediction and measurement Jacobians as follow
\begin{align}
	\boldsymbol{F}_k = \pabk{\fb}{\xb}_{\boldsymbol{\mu}_{k}^{-}}, \: \boldsymbol{H}_k = \pabk{\hb}{\xb}_{\boldsymbol{\mu}_{k}^{-}}.
\end{align}
%
Then by having the process and measurement Jacobians, the propagation of the error covariance matrix and calculation of the Kalman gain would be the same as the Kalman filter. Finally, we use the nonlinear models to find the mean of the state in the prediction and update step.\par
%
For the \enquote{prediction step} we have
%
\begin{align} 
\boldsymbol{\mu}_{k+1}^{-} &= \boldsymbol{f}(\boldsymbol{\mu}_{k}^+,
\boldsymbol{u}_{k}, 0) \nonumber\\
\boldsymbol{P}_{k+1}^{-} &= \boldsymbol{F}_k\boldsymbol{P}_k^{+}\boldsymbol{F}_k^T + \boldsymbol{Q}_k
\end{align}
%
and the \enquote{update step} is as follow
%
\begin{align}\label{eq:update_nonlinear}
\boldsymbol{K}_k &= \boldsymbol{P}_{k+1}^{-}\boldsymbol{H}_k^T (\boldsymbol{H}_k\boldsymbol{P}_{k+1}^{-}\boldsymbol{H}_k^T + \boldsymbol{R}_k)^{-1} \nonumber \\
\boldsymbol{P}_{k+1}^{+} &= (\boldsymbol{I} - \boldsymbol{K}_k\boldsymbol{H}_k) \boldsymbol{P}_{k+1}^{-}
\nonumber \\
\boldsymbol{\mu}_{k+1}^{+} &= \boldsymbol{\mu}_{k+1}^{-} + \boldsymbol{K}_k \left(\boldsymbol{y}_k - \boldsymbol{h}(\boldsymbol{\mu}_{k+1}^-, 0)\right).
\end{align}
%
This method is known as the \enquote{Extended Kalman Filter} (EKF). Although optimality and convergence are no longer guaranteed, this is a common approach for nonlinear estimation, and we choose the EKF over alternative frameworks such as the Unscented Kalman Filter \cite{Bloesch}, for its simplicity and low computational cost.\par

\section{Mathematical Derivation of EKF} \label{sec:math_ekf}
We would like to track the placement (position and orientation), and the linear velocity of an articulated floating base robot with two or more legs and equipped with an onboard IMU, joint sensing of position and torque. We adopt the estimation approach for extended Kalman filtering framework as presented in \cite{Camurri}. In this work, we focus on the Solo12 quadruped, but this approach is general and can be applied on the robots of the same type with differences only in the link lengths and sensor locations.

\subsubsection{Frames Definitions}
In~\figref{fig:frames} we illustrate the reference frames relevant to our estimation problem based on \cite{Camurri}. The inertial frame $W$ and the base frame $B$ are rigidly attached to the ground and the robot’s floating base, respectively. The IMU frame $I$ at the IMU sensor origin is also rigidly attached to the floating base. The relative placements (translation and orientation) of these frames are known by design, and they can be retrieved with respective transformation matrices. One or more temporal contact frame(s) $K$ are created when a foot comes into contact with the ground.\par
Reference frame conventions for a typical quadruped:
\begin{itemize}
	\item The world frame $W$ is fixed to the earth.
	\item The base frame $B$ is rigidly attached to the robot’s base.
	\item The IMU frame $I$ is rigidly attached to the robot’s chassis.
	\item The contact frame $K$, is perpendicular to the ground.
\end{itemize}
%
\begin{figure}[!htbp]
	\begin{center}
		\includegraphics[scale = 0.6]{Bilder/Frame_definition.pdf}
		\caption{Reference frame conventions for a typical quadruped.}
		\label{fig:frames}
	\end{center}
\end{figure}
%
\subsubsection{Notations}
In the remainder of this section, we adopt the following conventions; the robot position $\boldsymbol{p} = {^W}\boldsymbol{p}_{B} \in \mathbb{R}^3$ and orientation $\boldsymbol{q} = {^W}\boldsymbol{q}_{B} \in \mathit{S}^3$ (with the corresponding rotation matrix $\Rb = {^W}\Rb_{B} \in \mathit{SO}(3)$) are from world to base and expressed in world coordinates; the robot linear and angular velocities are respectively $\boldsymbol{v} = {^B}\vb_{WB}$, $\boldsymbol{\omega} = {^B}\boldsymbol{\omega}_{WB}$, and $\boldsymbol{v},\:\boldsymbol{\omega} \in \mathbb{R}^3$ from world to base and expressed in base coordinate; the IMU accelerometer and gyroscope biases are respectively ${^I}\boldsymbol{b}^a,\: {^I}\boldsymbol{b}^\omega \in \mathbb{R}^3$ expressed in IMU coordinate.
A time-dependent vector quantity $\boldsymbol{a}$ computed at time $t_k$ is shortened as $\boldsymbol{a}_k = \boldsymbol{a}(t_k)$.

\subsubsection{State Definition}
The robot state is defined as the vector concatenating position, orientation, linear velocity, and IMU biases. The angular velocity does not appear, as it is assumed to be directly measured by the IMU once the bias is properly compensated. The state at time $t_k$ is
\begin{align} \label{equ:state}
	\boldsymbol{x}_k = [\boldsymbol{p}_k, \boldsymbol{v}_k, \boldsymbol{q}_k, \boldsymbol{\omega}_k, \boldsymbol{b}^a_k, \boldsymbol{b}^\omega_k].
\end{align}

\subsubsection{Process Model}
The acceleration and angular velocity of the floating base are sensed by the IMU at high frequencies in the range 0.4–1 kHz. They are affected by bias and zero-mean Gaussian noise. By considering the offset between the base and IMU frame ${^B}\boldsymbol{r}_{IB}$, and the angular acceleration of the base (it can be computed by a simple finite differentiation from angular velocity); we can express the IMU sensed values in the base frame
\begin{align} \label{eq:imu}
	\boldsymbol{u}_t = 
		\begin{bmatrix}
		\boldsymbol{\omega}_t\\
		\boldsymbol{a}_t\\
	\end{bmatrix} = 
	\begin{bmatrix}
		{^B}\boldsymbol{R}_I(\boldsymbol{\Tilde{\omega}} - \boldsymbol{b}^\omega - \boldsymbol{\eta}^\omega)\\
		{^B}\boldsymbol{R}_I(\boldsymbol{\Tilde{a}} - \boldsymbol{b}^a - \boldsymbol{\eta}^a) + \boldsymbol{a}_{\alpha,\omega}\\
	\end{bmatrix}
\end{align}
where ${^B}\boldsymbol{R}_I$ is the rotational part of the rigid transform between the IMU and base frames, and $\boldsymbol{a}_{\alpha,\omega}$ is the effect of angular acceleration ($\boldsymbol{\alpha}$), and angular velocity ($\boldsymbol{\omega}$) of the floating base and is calculated as follow
\begin{align}
	\boldsymbol{a}_{\alpha,\omega} = (\boldsymbol{\alpha} \times {^B}\boldsymbol{r}_{IB}) 
	+ \boldsymbol{\omega} \times (\boldsymbol{\omega} \times {^B}\boldsymbol{r}_{IB}).
\end{align}
By taking time derivative from the states, we can find the non-linear dynamics. For the base position we have
%
\begin{align} \label{eq:position}
	\dot{\boldsymbol{p}} = {^W}\vb_{WB} = \Rb\vb.
\end{align}
%
By taking time derivative from~\eqref{eq:position}, we can find the ordinary differential equation for the base linear velocity
\begin{align}
	{^W}\dot{\vb}_{WB} &= \dot{\Rb}\vb + \Rb\dot{\vb} \rightarrow\nonumber \\
	\Rb\dot{\vb} &= -\dot{\Rb}\vb + {^W}\dot{\vb}_{WB} \rightarrow \nonumber \\
	\Rb\dot{\vb} &= -\Rb [\boldsymbol{\omega}]_{\times}\vb + {^W}\dot{\vb}_{WB} \rightarrow \nonumber \\
	\dot{\vb} &= \Rb^T\lef-\Rb [\boldsymbol{\omega}]_{\times}\vb + ({^W}\ab_{WB} + \gb)\rig \rightarrow \nonumber \\
	\dot{\vb} &= -[\boldsymbol{\omega}]_{\times}\vb + \boldsymbol{R}^T\boldsymbol{g} + {^B}\ab_{WB}.
\end{align}
The dynamics for the rotation part comes from \eqref{eq:ode}, and we consider random walk noise for the biases. Finally, the quantities from~\eqref{eq:imu} are used as inputs to the nonlinear continuous process model in the EKF
\begin{align} \label{eq:con_process_model}
	\dot{\boldsymbol{x}} = 
	\begin{bmatrix}
		\pbp\\
		\vbp\\
		\qbp\\
		\bbp^a\\
		\bbp^{\omega}\\
	\end{bmatrix} 
	= 
	\begin{bmatrix}
		\boldsymbol{R}\boldsymbol{v}\\
		-[\boldsymbol{\omega}_t]_\times \boldsymbol{v} + \boldsymbol{R}^T\boldsymbol{g} + \boldsymbol{a}_t\\
		\frac{1}{2} \boldsymbol{\Omega}_R(\boldsymbol{\omega}_t)\qb\\
		\boldsymbol{\eta}^a_b\\
		\boldsymbol{\eta}^\omega_b\\
	\end{bmatrix}.
\end{align}
Since \eqref{eq:con_process_model} is continuous and nonlinear, it must be discretized and linearized for the implementation purposes. Following the EKF framework outlined in section~\ref{sec:ekf}, this requires two systems of equations: (1) a discrete, nonlinear system for prediction of the mean of the state and measurement, and (2) a discrete, linear system for propagation of the state covariance matrix through the prediction model and for computation of the Kalman gain in the update step.\par

For the \enquote{prediction step}, given~\eqref{eq:con_process_model}, we can predict the mean of the state, $\boldsymbol{\mu}_k$, by simple first order Euler integration over the period $\Delta t = t_k - t_{k-1}$
\begin{align} \label{eq:dis_process_model}
	\boldsymbol{\mu^{-}_{k+1}} = 
	\begin{bmatrix}
		\boldsymbol{p}_k\\
		\boldsymbol{v}_k\\
		0\\
		\boldsymbol{b}^a_k\\
		\boldsymbol{b}^\omega_k\\
	\end{bmatrix} 
	+ 
	\begin{bmatrix}
		(\boldsymbol{R}_k\boldsymbol{v}_k)\Delta t\\
		(-[\boldsymbol{\omega}_k]_\times \boldsymbol{v}_k + \boldsymbol{R}_k^T\boldsymbol{g} + \boldsymbol{a}_k)\Delta t\\
		\boldsymbol{R}_k \exp([\boldsymbol{\omega}_k \Delta t]_\times)\\
		0\\
		0\\
	\end{bmatrix}.
\end{align}
Note that the attitude is integrated separately using the exponential map between the Lie group of rotations and its Lie algebra at the identity.\par

Recall that discretized, linearized dynamics are required in order to propagate the error estimate covariance matrix. We adopt the approach in \cite{Rotella}, and we first linearize and then discretize to get the continuous prediction Jacobian $\Fb_c$, and noise Jacobian $\Lb_c$
\begin{align}
	\Fb_c = 
	\begin{bmatrix}
		0 & \Rb & -\Rb[\vb]_\times & 0 & 0\\
		0 & -[\boldsymbol{\omega}]_\times & [\Rb^T\gb] & -\Ib & -[\vb]_\times\\
		0 & 0 & -[\boldsymbol{\omega}]_\times & 0 & -\Ib\\
		0 & 0 & 0 & 0 & 0\\
		0 & 0 & 0 & 0 & 0\\
	\end{bmatrix}, \: \Fb_c \in \mathbb{R}^{15\times 15}
\end{align}
\begin{align}
	\Lb_c = 
	\begin{bmatrix}
		0 & 0 & 0 & 0 \\
		-\Ib & -[\vb]_\times & 0 & 0\\
		0 & -\Ib & 0 & 0\\
		0 & 0 & \Ib & 0\\
		0 & 0 & 0 & \Ib\\
	\end{bmatrix}, \: \Lb_c \in \mathbb{R}^{15\times 12}.
\end{align}
It is assumed for simplicity that the covariance matrix of each process noise vector is diagonal with equal entries. The continuous process noise covariance matrix is then
\begin{align}
	\Qb_c = diag(\boldsymbol{Q}_a, \boldsymbol{Q}_\omega, \boldsymbol{Q}_{b_a}, \boldsymbol{Q}_{b_\omega}).
\end{align}
The covariance matrices, $\boldsymbol{Q}_a, \boldsymbol{Q}_\omega, \boldsymbol{Q}_{b_a}, \boldsymbol{Q}_{b_\omega}$ are obtained from \cite{Sola} by integrating the covariances of $\ab_n,\: \boldsymbol{\omega}_n,\: \ab^b_n,\: \boldsymbol{\omega}^b_n,\: $ over the step time $\Delta t$
\begin{align}
	\boldsymbol{Q}_a &= \sigma^2_{\ab_n} {\Delta t}^2 \Ib & &[\unit{m^2/s^2}]\\ 
	\boldsymbol{Q}_\omega &=\sigma^2_{\boldsymbol{\omega}_n}  {\Delta t}^2 \Ib & &[\unit{rad^2}]\\
	\boldsymbol{Q}_{b_a} &= \sigma^2_{\ab^b_n} {\Delta t} \Ib & &[\unit{m^2/s^4}]\\
	\boldsymbol{Q}_{b_\omega} &= \sigma^2_{\boldsymbol{\omega}^b_n} {\Delta t} \Ib & &[\unit{rad^2/s^4}]
\end{align}
where the corresponding covariances ($\sigma$) are to be determined from the information in the IMU data sheet, table~\ref{tab:imu_data}.
\renewcommand{\arraystretch}{1.3}
\begin{table}[!htbp]
	\caption{IMU sensor outputs.}\vspace{6pt}
	\label{tab:imu_data}
	\centering
	\begin{tabular}{p{3.5cm}p{3cm}p{3cm}p{3.2cm}}
		\hline
		\hline
		& Accelerometer & Gyroscope & Magnetometer \\
		\hline
		\hline
		Measurement range & $\pm8\:g$ & $300^{\circ}/\unit{sec}$ &  $\pm8\:\unit{Gauss}$ \\
		\hline
		Non-linearity & $\pm0.02\%\: \unit{fs}$ & $\pm0.02\%\: \unit{fs}$ & $\pm0.3\%\: \unit{fs}$ \\
		\hline
		Resolution & $<0.1\: mg$ & $<0.003^{\circ}/\unit{sec}$ & -- \\
		\hline
		Bias instability & $\pm 0.04\:mg$ & $8^{\circ}/\unit{hr}$ & -- \\
		\hline
		Initial bias error & $\pm 0.002\:g$ & $\pm0.04^{\circ}/\unit{sec}$ & $\pm0.003\:\unit{Gauss}$ \\
		\hline
		Noise density & $20\:\mu g/\surd{\unit{Hz}}$ & -- & $400\;\unit{\mu Gauss} /\surd{\unit{Hz}}$ \\
		\hline
		\hline
	\end{tabular}
\end{table}
\\Assuming a zero-order hold on inputs over the interval~$\Delta t = t_k - t_{k-1}$, the discretized prediction Jacobian and discretized error covariance matrix are given by \cite{VanLoan}
\begin{align}
	\Fb_k &= \exp(\Fb_c\Delta t)\\
	\Qb_{k-1} &= \int_{t_{k-1}}^{t_{k}} \exp(\Fb_c (t_k - \tau))\Lb_c\Qb_c\Lb_c^T\exp(\Fb_c^T (t_k - \tau)) d\tau.
\end{align}
In practice, these expressions are often truncated at first order for simplicity to yield \cite{Rotella}
\begin{align}
	\Fb_k &\approx \Ib + \Fb_c\Delta t\\
	\Qb_k &\approx (\Fb_k\Lb_c\Qb_c\Lb_c^T\Fb_k^T)\Delta t.
\end{align}
\subsubsection{Measurement Model}
The linear velocity of the base is computed using leg odometry. When a foot is in contact with the ground, as shown in the~\figref{fig:measurement_model} by simple vector addition we have
\begin{align} \label{eq:measurement}
	^{W}\boldsymbol{p}_{WB} + {^W}\Rb_{B} \lef {^B}\boldsymbol{p}_{BK_i}(\alpha)\rig = {^W}\rb_{WK_i}
\end{align}
where ${^B}\boldsymbol{p}_{BK_i}(\alpha)$ is the position of the foot $i$ relative to the base expressed in the base frame, which is a function of the joints angles of the corresponding leg.
%
\begin{figure}[!htbp] 
	\begin{center}
		\includegraphics[scale = 0.6]{Bilder/Measurement_model.pdf}
		\caption{Measurement model.}
		\label{fig:measurement_model}
	\end{center}
\end{figure}
%
By taking time derivative from~\eqref{eq:measurement}, and given the joint positions and velocities we can compute , using forward kinematics, the velocity and position of each end effector in the base frame. By assuming that the foot remains stationary while is in contact with the ground (${^W}\dot{\rb}_{WK_i} = 0$), the measurement model can be written as follow \cite{Camurri}
\begin{align} \label{equ:ekf_measurement}
	{^W}\dot{\rb}_{WK_i} = {^B}\boldsymbol{v}_{WB} + {^B}\boldsymbol{v}_{BK_i} + {^B}\boldsymbol{\omega}_{WB} \times {^B}\boldsymbol{p}_{BK_i}
\end{align}
where the angular velocity is measured directly by the IMU once the gyroscope bias was properly compensated. 
After determining the set of feet in contact with the ground (explained in details in the next section), the robot's base linear velocity can be measured from each foot in contact. By having the joint positions and velocities sensed from the encoders $\boldsymbol{q}$, $\dot{\boldsymbol{q}}$ and considering their additive noise $\boldsymbol{\eta}^q$, $\boldsymbol{\eta}^v$ the measurement model can be rewritten as follow
\begin{align}
	\Tilde{\boldsymbol{v}}_k = -\boldsymbol{J}(\Tilde{\boldsymbol{q}}_k - \boldsymbol{\eta}^q)(\Tilde{\dot{\boldsymbol{q}}}_k -\boldsymbol{\eta}^v) - \boldsymbol{\omega} \times f\!k(\Tilde{\boldsymbol{q}} - \boldsymbol{\eta}^q).
\end{align}
We collect all the effects of noise into one additive term $\boldsymbol{\eta}^v$
\begin{align}
	\Tilde{\boldsymbol{v}}_k = -\boldsymbol{J}(\Tilde{\boldsymbol{q}}_k)\Tilde{\dot{\boldsymbol{q}}}_k - \boldsymbol{\omega} \times f\!k(\Tilde{\boldsymbol{q}}) + \boldsymbol{\eta}^v.
\end{align}
First, we compute $\Tilde{\boldsymbol{v}}_k$, the measured base linear velocity, from forward kinematics of each foot in contact. Then by linearizing the measurement model~\eqref{equ:ekf_measurement} we can find the continuous measurement Jacobian
\begin{align}
	\Hb_c = 
	\begin{bmatrix}
		0 & \Ib & 0 & 0 & [{^B}{\Sb}_{BF_1}]_{\times} \\
		0 & \Ib & 0 & 0 & [{^B}{\Sb}_{BF_2}]_{\times} \\
		0 & \Ib & 0 & 0 & [{^B}{\Sb}_{BF_2}]_{\times} \\
		0 & \Ib & 0 & 0 & [{^B}{\Sb}_{BF_2}]_{\times} \\
	\end{bmatrix}, \: \Hb_c \in \mathbb{R}^{12\times 15}
\end{align}
which ${^B}{\Sb}_{BF_i}$ corresponds to the position of the foot $i$ (in contact with the ground) relative to the base frame, expressed in the base frame. Note that, if the foot $i$ is not in contact with the ground, the corresponding row in the measurement Jacobian would be zero. The measurement model has algebraic dynamics, therefore the measurement Jacobian and the measurement noise covariance matrices are the same for continuous and discrete-time systems
\begin{align}
	\Hb_k &= \Hb_c\\
	\Rb_{k} &= \Rb_c.
\end{align}
The measurement noise covariance matrix $\Rb_c$ should be determined experimentally in order to have an optimal performance, and it can be seen as a tuning knob in the EKF which handles the uncertainty in the measurement model , and it is also one way to incorporate the uncertainty in contact detection and slippage of the foot in contact in our estimation framework.\par

Eventually, by having $\Hb_k$, and $\Rb_k$ we apply the update step of the EKF as described in~\eqref{eq:update_nonlinear} to get the \enquote{a posteriori} estimates.

\section{Contact Detection} \label{sec:contact}
Quadruped robots are typically equipped with high-precision joint encoders from which low-noise joint velocity measurements can be derived. However, achieving accurate contact estimation is a major challenge since quadrupeds are not typically equipped with direct contact sensors, as they easily break during operation.\par

Quadruped robot feet are usually approximated to be point-like and then assumed to exert only pure forces onto the ground. By assuming that the robot base remains flat during contact transitions, we can consider an equal distribution of the robot total weight over the feet in contact with the ground. We use a Schmitt trigger to implement a robust hysteresis on the contact decision. we choose upper and lower bounds as two thresholds for contact classification based on measured force for each end effector. If the norm of the force at each end effector is higher than the upper threshold, we consider the foot as in contact with the ground, and if the norm is below the lower threshold, the end effector is no longer in contact. This allows some hysteresis in the contact detection which helps reject outliers due to high joint acceleration when the foot leaves the ground. We compute the end effector force norm using the joint torque as follows
\begin{align} \label{eq:contact_detection}
	\boldsymbol{F}_i &= (\boldsymbol{S}_i \boldsymbol{J}_i^T)^{-1} \boldsymbol{S}_i \boldsymbol{\tau} \nonumber \\
	F_i &= ||\boldsymbol{F}_i||
\end{align}
where $\boldsymbol{S}_i$ is the selection matrix for the joints of leg $i$.
%
\section{Implementation} \label{sec:implementation}
In this section we present the overview of the implemented EKF, and briefly mention the simulation environment (used for debugging and testing), real hardware and related open source software.
%
\subsubsection{Software Structure}
The schematic of EKF is demonstrated in~\figref{fig:ekf_fig}.
\begin{figure}[!htbp]
	\begin{center}
		\includegraphics[scale = 0.45]{Bilder/EKF.pdf}
		\caption{A summary of the extended Kalman filter.}
		\label{fig:ekf_fig}
	\end{center}
\end{figure}

\subsubsection{Software Packages}
We are using \textsc{Pinocchio} \cite{Pinocchioweb} for computing the kinematics and dynamics of a robot model. \textsc{Pinocchio} instantiates the state-of-the-art \enquote{Rigid Body Algorithms} for poly-articulated systems based on revisited \cite{Featherstone} algorithms. \textsc{Pinocchio} is a \textsc{C++} modeling library with \textsc{Python} bindings.
\subsubsection{Simulation}
The physics simulator used for this project is \textsc{Pybullet} \cite{coumans2020}. We use simulation results to validate the estimation framework before implementing on the real hardware.

\subsubsection{Real Hardware}
On the real hardware, the framework is designed using Robot Operating System (\textsc{Ros2}) as its middle-ware to communicate with robot and perception services. \textsc{Ros} provides different set of programs, tools and libraries for the development of robotic software application. It has been open sourced, and a huge community constantly contributes to the development of different support packages. The complex framework, is simplified by providing tools for hardware abstraction, management, driver tools and visualizer to easy work on different applications. Moreover, the software architecture and tool kits for the overall framework are available open source in GitHub repository of \cite{MinM}.