# State Estimation of Over-Sensored Systems Applied to a Low-Cost Robotic Manipulator
## 2 Models
### Kalman Filter  
Once a system is modeled, its state can be approximated according to measurements taken from sensors. The state is a vector composed of all variables of interest of the system, and the way it is approximated is with sensor fusion techniques. The techniques described below assume that the state of the system is a first-order Markov process. This means that the next state will depend on the current state, the system inputs, and some noise. Similarly to the next state, the measurements obtained from sensors are dependent on the state, the inputs, and some noise. Generically, a digital system can be mathematically described by:  
\[x(k+1) =f(x(k),u(k),w(k),k) \tag{18}\] \[y(k) =g(x(k),u(k),v(k),k) \tag{19}\]  
where \(x\) is the state, \(u\) the set of inputs, \(y\) the set of outputs, \(w\) the process noise, and \(v\) the measurement noise. It should be noted that the system may be dependent on time \(k\), which means it is not necessarily time invariant. To estimate the state of a system, the Kalman Filter (KF) makes some assumptions. The first is the linearity of the system, and the second is related to the process and measurement noises, both assumed to be zero-mean white noise stochastic variables. Under this assumptions, Equations (18) and (19) can be rewritten. \[x(k+1) =A(k)x(k)+B(k)u(k)+w(k) w(k) \sim N(0,Q(k)) \tag{20}\] \[y(k) =C(k)x(k)+D(k)u(k)+v(k) v(k) \sim N(0,R(k)) \tag{21}\]  
where \(N(\mu,\Sigma)\) is a Gaussian distribution of mean \(\mu\) and covariance matrix \(\Sigma\). Once again, it should be noted that the system matrices (\(A\), \(B\), \(C\), and \(D\)) are not time invariant. In the case of the robotic manipulator, the system must be considered time variant, because different joint angles produce different transformations between the referentials of each link. If these assumptions are valid, the KF is the minimum-variance unbiased estimator [10] that gives the most certain estimate out of the ones that do not produce a systematic error. The Kalman filter's algorithm is usually implemented in 2 stages: prediction and filtering. Figure 1: Example of a coordinate transformation with Denavit–Hartenberg (DH) parameters. The prediction stage uses the model of the system and the current state (or its estimate) to guess what the next state might be. Besides that, it determines the level of certainty of that guess in the form of a covariance matrix. In the filtering stage, measurements are used to correct the guess of the prediction stage. The influence the measurements have in the final estimate depend on the previously calculated covariance matrix and the certainty granted to the measurements. A covariance matrix of the final estimate is also calculated as it is needed in the prediction stage. Assuming the process noise and the measurement noise are uncorrelated (which is the most common situation), the estimator is implemented with the following steps [11]:  
Prediction:  
\(\hat{x}(k+1|k)=A(k)\hat{x}(k|k)+B(k)u(k)\) (next state estimate)  
\(P(k+1|k)=A(k)P(k|k)A(k)^{T}+Q(k)\) (next state estimate's covariance)  
Filtering:  
\(e(k)=y(k)-C(k)\hat{x}(k|k-1)-D(k)u(k)\) (innovation)  
\(S(k)=R(k)+C(k)P(k|k-1)C(k)^{T}\) (innovation's covariance)  
\(K(k)=P(k|k-1)C(k)^{T}\mathcal{S}(k)^{-1}\) (Kalman gain)  
\(\hat{x}(k|k)=\hat{x}(k|k-1)+K(k)e(k)\) (current state estimate)  
\(P(k|k)=(I-K(k)C(k))P(k|k-1)\) (current state estimate's covariance)  
In this algorithm, \(\hat{x}\) is a state estimate, and the notation \((\alpha|\beta)\) refers to the instant at which the estimate corresponds (\(\alpha\)) given measurements up to time \(\beta\). The variables calculated in Steps 1, 2, and 3 of the filtering stage are merely auxiliary. The innovation \(e(k)\) is the error in the measurements, i.e., the difference between the expected measurements and the ones actually obtained.