Can you elaborate on your question? 

The EKF is as generic as you need. You can feed the EKF with whatever observation model you want, so you can devise the model that relates your arm orientation sensors to your arm motion model and feed the EKF with measurements.

Hello

I second the previous poster's request. All the EKF does is take a measurement sequence from a nonlinear dynamical system and give you a sequence of state estimates, filtering the measurements sequentially. Fusion means you are taking measurements from more than 1 sensor, all of which relate to the state, and stacking them into a combined measurement vector. The problem is harder if the measurements have different coordinate systems, correlated noise, lack of synchronisation, data ambiguity, etc.

The papers I have found show no samples, but instead, just math formulas with complex equations. So that's my difficulty. I Just want to implement in C# an extended kalman filter algorithm, and feed with informations (based on quaternions) from two different sensors like: X1 = [Q1, Q2, Q3, Q4]; X2 = [Q1, Q2, Q3, Q4]. Firstly I want to make a data fusion with X1 and X2. Then, I just want to feed the Extended Kalman Filter in a most properly way possible. Did you understand? I want the clearest samples, like a pseudocode, an algorithm implementation or at least a simple math equation explaining about this. Thank you guys!

Basically, you'll have two main methods:

xpk+1 = prediction(xk, Ak)

that will predict your state at the next time sample based on the current state estimate xk and on the state transition matrix Ak, that you can derive from your system dynamics. Note the under-script k means this can be time dependent if your system is non-linear. In this case you'll have to linearize the state transition matrix around xk.

and

xk+1 = update(xpk+1, H, z)

that will update your state estimate prediction xpk+1 based on the sensors measurement model H and on the measurements from these sensors, z.

You can call the prediction step at a fixed timestep, that will dictate your state estimation frequency, You call the update step whenever you get new measurements.

The complex equations you are referring to are probably the linear algebra that goes inside these two methods. In the prediction step, you need to propagate the state and covariance, in update step you have to compute the innovations and the Kalman gain and then use them to update the state estimate and covariance matrix. You can easily compute these using some basic linear algebra library for your preferred computing language (search for UBLAS).

Sorry for being slow to answer you Morteza, I have a lot of references about this, but I think the papers from the links below show the main idea of my work:

A comparison of unscented and extended Kalman filtering for estimating quaternion motion

Orientation Estimation by Error-State Extended Kalman Filter in Quaternion vector space

http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=1243440&url=http%3A%2F%2Fieeexplore.ieee.org%2Fxpls%2Fabs_all.jsp%3Farnumber%3D1243440

http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4420951&url=http%3A%2F%2Fieeexplore.ieee.org%2Fxpls%2Fabs_all.jsp%3Farnumber%3D4420951

dear carlos

this is a link for ieee paper just from 2015 American Control Conference

the abstract is great you can read it first:

Measurement censoring, or Tobit model censoring, is common in many engineering applications. It arises from limits in sensor dynamic range, and may be exacerbated by poor calibration of sensors. Censoring is often referred to as a clipped measurement or limit-of-detection discontinuity, and is represented as a piecewise-linear transform of the output variable. The slope of the piecewise-linear transform is zero in the censored region. This form of nonlinearity presents significant challenges when a nonlinear approximation to the Kalman filter is to be used as an estimator. The Tobit Kalman filter is a new method that is a computationally efficient, unbiased estimator for linear dynamical systems with censored output. In this paper, we use Monte Carlo methods to compare the performance of the Tobit Kalman Filter to the performance of the Extended Kalman Filter and the Unscented Kalman Filter. We show that the Tobit Kalman Filter reliably provides accurate estimates of the state and state error covariance with censored measurement data, while both the EKF and the UKF provide unreliable estimates in censored data conditions

Best regards

Ashraf Abosekeen

http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=7172142&newsearch=true&queryText=extended%20kalman%20filter%20fororientation%20data

Thank you by your referece Ashraf Abosekeen!

Hey Haykel Snoussi, can you show me your technical report? I will be thankful!