# State Estimation of Over-Sensored Systems Applied to a Low-Cost Robotic Manipulator
## 4 State Estimation
### Link Model Estimation
#### 4.1.1 Gyroscope Model  
Using just the gyroscopes, the orientation of Link 1 remained static throughout the execution of the test. This is a consequence of the lack of an IMU in that link. Under these circumstances, the orientations of each link are independent. Therefore, there is nothing preventing adjacent links from being rotated by an axis different than the one allowed by the joints. This is evident in the graph of Figure 6.  
This graph shows the evolution of the rotation vector \(\theta_{2}\) and the baseline for the first two angles (\(\theta_{1b}\) and \(\theta_{2b}\)). Note that \(\theta_{1b}\) was included here since there would be no relation between this angle and the one estimated by the link model as the estimated orientation of Link 1 is static.  
However, there should be a relation between \(\theta_{1b}\) and \(\theta_{2x}\), because when \(\theta_{2}=0\), a rotation of \(\theta_{1}\) is equivalent to a rotation of Link 2 around its \(x\) axis. This association is evident in the time intervals when \(\theta_{1}\
eq 0\), that is \([15;40]\) s. Note this relation is even stronger in the interval \([20;30]\) s, which is when \(\theta_{2}=0\).  
The relation between \(\theta_{2b}\) and \(\theta_{2z}\) is also evident in the intervals \([5;20]\) s and \([30;45]\) s. Again, because the rotation vector is not always pointing along the \(z\) axis of Frame 2, the association is mostly present when \(\theta_{1}=0\), which is in intervals \([5;15]\) s and \([40;45]\) s. However, even in these intervals, the value of \(\theta_{2z}\) seems to be slightly higher than that of \(\theta_{2b}\) by about \(5^{\circ}\). This slight overshoot is actually visible when performing the test, which means the gyroscope is more accurate than the encoders. The reason the encoder measures a smaller value than the real one is that the movement performed at instants 5 s and 30 s is so fast that the encoder misses some pulses. This kind of error is correctable by the other sensors.  
In the moments when \(\theta_{1}\
eq 0\) and \(\theta_{2}\
eq 0\), the rotation vector points along a direction that also has a \(y\) component, which is visible in the intervals \([15;20]\) s and \([30;40]\) s. These also correspond to the intervals when there is not a direct correlation between the baseline variables and the corresponding rotation vector components.  
Figure 7 shows the graph with the components of Rotation Vector 3 (\(\theta_{3x}\), \(\theta_{3y}\), and \(\theta_{3z}\)) and the baseline for the angle of the third joint (\(\theta_{3b}\)). Assuming the orientation of Link 2 is being correctly estimated, this rotation vector should only have a \(z\) component. As expected, \(\theta_{3z}\) roughly matches \(\theta_{3b}\).  
Figure 6: Evolution of Rotation Vector 2 when testing the link model using only gyroscope measurements.  
However, the \(x\) and \(y\) components of the rotation vector are still significant during some periods. This discrepancy starts to be noticeable at 15 s, which is exactly when the first joint starts rotating out of the \(0^{\circ}\) angle. This error is justifiable by a slight tilt of the manipulator in one direction that should not be allowed by the manipulator. Nevertheless, it is relevant to have the model notice these imprecisions, as they influence the estimate of the position of the manipulator's tip. The reason the error only appears at that moment is related to the joint that moves. Once \(\theta_{1}\) varies, inertia in the manipulator can cause it to tilt to a different side from the original. Nevertheless, the \(x\) and \(y\) components are small when compared to the length of the rotation vector, which is approximately equal to \(\theta_{3x}\).