# State Estimation of Over-Sensored Systems Applied to a Low-Cost Robotic Manipulator
## 2 Models
### Kalman Filter
#### 2.4.1 Extended Kalman Filter  
As mentioned, one of the variants of the KF is the extended Kalman filter. This variant starts by finding the required Jacobian matrices (matrices composed of partial derivatives) and uses them to describe an approximation of the system, then it solves the estimation problem for the approximate system with a conventional KF.  
\[A(k)=\frac{\partial f}{\partial x}(k)\hskip 28.452756 ptB(k)=\frac{\partial f}{ \partial u}(k)\hskip 28.452756 ptC(k)=\frac{\partial g}{\partial x}(k) \hskip 28.452756 ptD(k)=\frac{\partial g}{\partial u}(k) \tag{22}\]  
This simple workaround removes the assurance that this is the minimum-variance unbiased estimator. However, it produces good enough results to be used in many practical applications.  
Even though this filter is usable in the context at hand, it presents some issues when used to estimate orientations. The main problem lies in the way that orientation is rep resented. For example, when the orientation of a body is described by a rotation matrix, then the correction of this orientation with a measurement, which is performed in Step 4 of the filtering stage, is the sum of two rotation matrices. This sum most likely is not a valid rotation matrix (i.e., is not orthogonal). This means that representing an orientation by the sum of its estimate with its error is not a valid model.  
\[R_{\theta}=R_{\hat{\theta}}+R_{\epsilon} \tag{23}\]  
This makes it difficult to apply the filtering step of the Kalman filter. One way to work around this problem is to assume that Equation (23) is valid and to alter the resulting matrix to become orthogonal again. If the orientation was being described by unit quaternions, this process would be even simpler, as the only change needed to turn the quaternion into a valid orientation would be to divide it by its magnitude. However, letting the filter assume that rotation matrices can simply be summed produces an incorrect model of the system, which in turn causes the filter to have a wrong sense of certainty about the state of the system [15].  
A different way to model the dynamics of an orientation is through matrix multiplication.  
\[R_{\theta}=R_{\theta}R_{\eta} \tag{24}\]  
This equation always produces at its output a valid rotation matrix (assuming there are no rounding errors), as long as the error matrix (\(R_{\eta}\)) is a rotation matrix as well. \(R_{\eta}\) can be made into a valid rotation matrix if it is built by an orientation representation that is not redundant (i.e., has no constraints), for example a rotation vector \(\eta\). The conversion between rotation vector and rotation matrix can be performed using matrix exponentiation [11; 15]. This approach to orientation estimation is called the Multiplicative Extended Kalman Filter (MEKF). This extra step in the MEKF is called the reset step, and it is what allows for the orientation to be stored in the rotation matrix while the filter operations are applied to the rotation vector. This filter is often used in spacecraft orientation estimation [16], which proves this filter's worth. Its intricacies will be further explored when the robotic manipulator model is explained.