# State Estimation of Over-Sensored Systems Applied to a Low-Cost Robotic Manipulator
## 3 Test Setup
### Models
#### 3.3.1 Link Model  
As mentioned, in this model, the state is composed of the rotation matrices \({}_{n}^{i}R\) and angular velocities \(\omega_{i}\) of each link. Each of these represents the orientation of link \(i\) (with 0 being the base) with respect to an inertial frame \(n\). The inertial frame is denoted by \(n\) because it refers to the navigation frame. This is a frame situated at a specific point on Earth's surface. As the planet rotates, so does the frame. Even though this frame is not inertial, it can be considered approximately inertial. As demonstrated in [15], the Coriolis and centrifugal accelerations suffered by this frame are negligible. The choice of representing orientations with respect to an inertial frame means that any error in estimating the orientation of any link does not propagate to the next one. It also means that inertial measurements from the IMU will be easier to model as they are independent of the previous link. On the other hand, this will make sensors like encoders, which depend on the orientation of two links, have a more complex model. The rotation matrix for each link evolves according to the following equation:  
\[{}_{i}^{i}R(k+1)={}_{n}^{i}R(k)R_{\delta_{i}}(k) \tag{26}\]  
where \(R_{\delta_{i}}(k)\) is the rotation matrix corresponding to the rotation vector \(\delta_{i}(k)\). \(\delta_{i}(k)\) is the vector around which frame \(i\) rotates from instant \(k\) to instant \(k+1\). The way the rotation matrix can be built from the rotation vector is through matrix exponentiation. The value of \(\delta_{i}(k)\) is given by:  
\[\Delta_{i}(k)=\Delta t(\omega_{i}(k)+\epsilon_{\omega_{i}}(k))=\Delta t\omega_ {i}(k)+\epsilon_{\delta_{i}}(k) \tag{27}\]  
with \(\Delta t\) the time interval between instants \(k\) and \(k+1\), \(\omega_{i}={}^{i}\omega_{ni}\) the angular velocity of link \(i\) with respect to frame \(n\) represented in frame \(i\), and \(\epsilon\) the error of the model. One filter designed to find the orientation of a rigid body using an IMU is the MEKF. As mentioned in Section 2.4.1, Model (26) must be linearized in order to apply the MEKF. With that intent, the rotation matrix \({}_{i}^{i}R\) is rewritten such that the state becomes a vector around an operating point. \[{}_{n}^{i}R(k)={}_{n}^{i}\hat{R}(k)R_{\eta_{i}}(k) \tag{28}\]  
In this equation, \({}_{n}^{i}\hat{R}\) is the rotation matrix representing the operating point, and \(R_{\eta_{i}}\) is the rotation around the operating point defined by the rotation vector \(\eta_{i}\). \(R_{\eta_{i}}\) can be approximated by:  
\[R_{\eta_{i}}(k)\approx I_{3}+[\eta_{i}(k)\times] \tag{29}\]  
where \([\ \cdot\ \times]\) is an operator that gives a \(3\times 3\) matrix from a vector. This operator can be used to solve a cross product:  
\[a\times b=[a\times]b=-[b\times]a \tag{30}\]  
and therefore is defined as  
\[[a\times]= \begin{bmatrix}0&-a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0\end{bmatrix}  \tag{31}\]  
with \(a=[a_{1}\quad a_{2}\quad a_{3}]^{T}\). Given Equation (29), the linear model can be deduced:  
\[\eta_{i}(k+1)=\hat{R}_{\delta_{i}}^{T}(k)\eta_{i}(k)+\Delta t\epsilon_{\omega_ {i}}(k) \tag{32}\]  
Note that this model is not required for the update stage of the MEKF because \(\eta_{i}\) and \(\epsilon_{\omega_{i}}\) are zero-mean variables. This model is only needed to find the matrices required for the filtering stage. Finally, the angular velocity \(\omega_{i}\) is modeled as a random walk that is corrected by measurements:  
\[\omega_{i}(k+1)=\omega_{i}(k)+\epsilon_{\omega_{i}}(k) \tag{33}\]_Gyroscope Model_  
Once properly calibrated, each gyroscope should give a direct measure of one of the state variables (per link). Its measurement model is defined by the following equation:  
\[\omega_{Si}(k)=\omega_{i}(k)+\epsilon_{\omega_{Si}}(k) \tag{34}\]  
which is already linear. _Magnetometer Model_  
When considering a sensor like the magnetometer, all one needs to take into consideration is the orientation of the vector being measured, i.e., the magnetism vector.