# State Estimation of Over-Sensored Systems Applied to a Low-Cost Robotic Manipulator
## 3 Test Setup
### Models
#### 3.3.2 Joint Model
However, given the relation:  
\[{}^{i}\omega_{mi}(k)=R_{\theta_{i}}^{T}(k)\,{}^{i-}_{i-1}R^{T}\,{}^{i-1}\omega _{n(i-1)}(k)+{}^{i}\omega_{(i-1)i}(k) \tag{48}\]  
the gyroscope measurements can be written as a function of the angular velocity of the joint (\({}^{i}\omega_{(i-1)i}\)), the angle of the joint (which affects \(R_{\theta_{i}}^{T}\)), and the angular velocity of the previous link with respect to the inertial frame (\({}^{i-1}\omega_{n(i-1)}\)). With this relation, the gyroscope model can be deduced. \[\omega_{g_{i}}(k)={}^{i}\omega_{ni}(k)+\epsilon_{g_{i}}(k) \tag{49}\]  
with \(\epsilon_{g_{i}}(k)\) the gyroscope measurement noise and \({}^{i}\omega_{(i-1)i}(k)= \begin{bmatrix}0&0&\dot{\theta}_{i}(k)\end{bmatrix} ^ {T}\). Accelerometer Model  
The acceleration of a joint can be written as a function of the acceleration of the previous joint:  
\[{}^{n}a_{i}(k)={}^{i-1}_{n}R(k)\,{}^{i-1}\Omega_{n(i-1)}(k)\,{}^{i-1}t_{i}+{}^ {n}a_{i-1}(k) \tag{50}\]  
with \({}^{i-1}t_{i}\) the translation vector from joint \(i-1\) to joint \(i\) in frame \(i-1\) and:  
\[{}^{i-1}\Omega_{n(i-1)}(k)=[{}^{i}\alpha_{ni}(k)\times]+[{}^{i}\omega_{ni}(k) \times]^{2} \tag{51}\]  
From this relation, the specific force in a joint can be determined:  
\[{}^{n}f_{i}(k)={}^{i-1}_{n}R(k)\,{}^{i-1}\Omega_{n(i-1)}(k)\,{}^{i-1}t_{i}+{}^ {n}f_{i-1}(k) \tag{52}\]  
Rotating it to the joint's frame:  
\[{}^{i}f_{i}(k)=R_{\theta_{i}}^{T}(k)\,{}^{i-}_{i-1}R^{T}\,{}^{i-1}\Omega_{n(i -1)}(k)\,{}^{i-1}t_{i}+{}^{i-1}f_{i-1}(k)\Big{)} \tag{53}\]  
The matrix \({}^{i-1}\Omega_{n(i-1)}(k)\) is a function of the angular velocity and angular acceleration of frame \(i-1\) with respect to frame \(n\). However, the state is composed of variables that describe the angular velocity and angular acceleration of the joints. In order to write the accelerometer measurement as a function of the state variables, the angular velocity and angular acceleration are rewritten:  
\[{}^{i}\alpha_{ni}(k)=R_{\theta_{i}}^{T}(k)\,{}^{i-}_{i-1}R^{T}\,{}^{i-1}\alpha _{n(i-1)}(k)+{}^{i}\alpha_{(i-1)i}(k) \tag{54}\]  
\[{}^{i}\omega_{ni}(k)=R_{\theta_{i}}^{T}(k)\,{}^{i-}_{i-1}R^{T}\,{}^{i-1}\omega _{n(i-1)}(k)+{}^{i}\omega_{(i-1)i}(k) \tag{55}\]  
with \({}^{i}\omega_{(i-1)i}(k)= \begin{bmatrix}0&0&\dot{\theta}_{i}(k)\end{bmatrix} ^ {T}\) and \({}^{i}\alpha_{(i-1)i}(k)= \begin{bmatrix}0&0&\ddot{\theta}_{i}(k)\end{bmatrix} ^ {T}\). Note that the influence of the angular acceleration in this model is what created the necessity for it to be included in the state vector. With these three recursive expressions, the accelerometer can now be modeled. \[f_{a_{i}}(k)={}^{i}\Omega_{ni}(k)\,{}^{i}p_{a_{i}}+{}^{i}f_{i}(k)+\epsilon_{a _{i}}(k) \tag{56}\]  
with \({}^{i}p_{a_{i}}\) the position of the accelerometer in the link and \(\epsilon_{a_{i}}(k)\) the accelerometer measurement noise. 3.3.3. Application of the Models to the Manipulator  
The models presented thus far are all symbolic and serve for all manipulators that follow the constraints initially mentioned. To make practical use of these models, the symbolic variables must be replaced by the appropriate constants for a specific manipulator. The manipulator is represented in Figure 4 along with the frames of each joint. From this representation, the constant rotation matrices can be found. Frames 0 and 1\(-\) are aligned, as are are Frames 2 and 3\(-\). This means the rotation matrices between these frames are the identity. \[ \begin{matrix}\begin{matrix}\begin{matrix}1\\ 0\end{matrix}\end{matrix}_{0}R=\begin{matrix}\begin{matrix}3\\ 2\end{matrix}\end{matrix}_{2}R=I_{3}\end{matrix}  \tag{57}\]  
In fact, if all joints have their angle at zero, the only referentials that are misaligned are 1 and 2\(-\). Using the Denavit-Hartenberg (DH) method, the rotation matrix \( \begin{matrix}\begin{matrix}2\\ 1\end{matrix}\end{matrix} _{1}R\) is the result of a rotation of \(-90^{\circ}\) around \(z\) followed by a rotation of \(-90^{\circ}\) around \(x\).