# State Estimation of Over-Sensored Systems Applied to a Low-Cost Robotic Manipulator
## 3 Test Setup
### Models
#### 3.3.2 Joint Model  
The joint model describes the pose of the manipulator by the angles \(\theta_{i}(k)\), angular velocities \(\dot{\theta}_{i}(k)\), and angular accelerations \(\ddot{\theta}_{i}(k)\) of each joint. These variables define the orientation of each joint relative to its previous joint instead of the \(n\) frame. Given that the angular acceleration \(\ddot{\theta}_{i}(k)\) is the derivative of the angular velocity \(\dot{\theta}_{i}(k)\) and that the angular velocity \(\dot{\theta}_{i}(k)\) is the derivative of the angle \(\theta_{i}(k)\), the model can be immediately written. \[\theta_{i}(k+1)=\theta_{i}(k)+\Delta t\dot{\theta}_{i}(k)+\frac{\Delta t^{2}}{ 2}\ddot{\theta}_{i}(k)+\epsilon_{\dot{\theta}_{i}}(k) \tag{41}\]  
\[\dot{\theta}_{i}(k+1)=\dot{\theta}_{i}(k)+\Delta t\ddot{\theta}_{i}(k)+ \epsilon_{\dot{\theta}_{i}}(k) \tag{42}\]  
\[\ddot{\theta}_{i}(k+1)=\ddot{\theta}_{i}(k)+\epsilon_{\ddot{\theta}_{i}}(k) \tag{43}\]  
In this model, \(\Delta t\) is once again the time interval between the instants \(k\) and \(k+1\). One advantage of this model over the link model is that its state vector is much smaller, with only three variables per joint. Furthermore, it is a linear model. However, because the position of a specific point in the manipulator depends on the orientation of all the preceding joints, the error of each joint builds up when estimating that position. Regarding the sensor models, the advantages of this model are the disadvantages of the previous one, and vice versa. Encoders will have a much simpler model as they give direct measurements of the state, whereas inertial measurements will depend on the states of all preceding joints. Lastly, this model cannot be used to know the pose of the manipulator with respect to an inertial frame if the relation between the base frame (0) and frame \(n\) is not known. Throughout the rest of this section, the base is considered fixed relative to frame \(n\) to simplify the model. The linearization of the models presented in this section is not calculated. This is due to the recursive nature of the models of the inertial sensors. This aspect produces complicated formulas that are configuration dependent and difficult to implement and debug. The lack of partial derivatives for those models prohibits the use of the EKF. Instead, the UKF should be used to estimate the state for this model. _Encoder Models_  
Given the nature of the chosen state, the encoder model is very simple:  
\[\omega_{i\epsilon_{i}}(k)=\dot{\theta}_{i}(k)+\epsilon_{i\epsilon_{i}}(k) \tag{44}\]  
with \(\epsilon_{i\epsilon_{i}}(k)\) the incremental encoder measurement noise. _Magnetometer Model_  
Under the same assumptions as the link model, a magnetometer's measurements can be considered to be constant in the neighborhood of the manipulator. This means that, once again, the measurements can be considered to be obtained in the joints' frames. Given this assumption, the following can be written. \[{}^{i}m(k)=R_{\theta_{i}}^{T}(k)\,{}^{i-}_{i-1}R^{T\,i-1}m(k) \tag{45}\]  
Knowing that the measurements are noisy:  
\[m_{m_{i}}(k)={}^{i}m(k)+\epsilon_{m_{i}}(k) \tag{46}\]  
This equation, along with the previous one, defines a recursive model that can be applied as long as the magnetism vector is known in any specific link. For example, if the magnetism vector in the base (Link 0) is known, the measurement from the magnetometer in Link 2 is given by:  
\[ \begin{split} m_{m_{2}}(k)&=R_{\theta_{2}}^{T}(k)\,{}^{ 2}_{-1}R^{T}\,{}^{1}m(k)+\epsilon_{m_{2}}(k)\\ &=R_{\theta_{2}}^{T}(k)\,{}^{2}_{-1}R^{T}R_{\theta_{1}}^{T}(k)\,{ }^{1}_{0}R^{T}\,{}^{0}m+\epsilon_{m_{2}}(k)\end{split}  \tag{47}\]  
Gyroscope Model  
The angular velocity measured by a gyroscope is always done with respect to an inertial frame.