1. INTRODUCTION
To capture human body motion in an ambulatory situation without the need for external emitters or cameras, it is possible to use inertial sensors such as gyroscopes, ultrasonic velocity sensors, radar altimeters and accelerometers to estimate the relative position and orientation (Morris (Reference Morris1973), Bonato (Reference Bonato2005), Foxlin (Reference Foxlin1996), Bachmann (Reference Bachmann2000), Molet, Boulic, and Thalmann (Reference Molet, Boulic and Thalmann1999)). Magnetic sensors can enhance the accuracy of these estimates and provide stability in the horizontal plane by sensing the direction of the earth's magnetic field (Zhu and Zhou (Reference Zhu and Zhou2004)). However in many situations involving prosthetic limbs it is not always possible to use magnetic sensors. A suitable alternative is to use satellite aided systems in much the same way as a TOM-TOM helps one to navigate on a motorway. Prosthetic limbs can be modelled as articulated rigid bodies in which the joints only have rotational degrees of freedom; this is unlike human body joints which cannot be modelled as a pure kinematic chain with well-defined joints such as hinge-joints (Zatsiorsky (Reference Zatsiorsky1998)). While satellite based navigation systems have been used to measure the relative attitude of a body by employing carrier phase measurements, it is not clear if code based position measurements could be employed to estimate the relative position and orientation of articulated prosthetic limb joints. Current practical schemes that are being developed use either rate gyroscopes or magnetic sensors (Giansanti et al. (Reference Giansanti, Macellari, Maccioni and Macellari2005), Roetenberg, Slycke, and Veltink (Reference Roetenberg, Slycke and Veltink2007) ). While the former are expensive the latter can be restrictive, although they can provide extremely accurate estimates of relative position and orientations.
Generally the rate gyros are the most expensive sensors and in an inertial navigation system we plan to employ only low cost MEMS accelerometer such as the ADXL203, two axis accelerometers manufactured by Analog Devices Inc., to provide full inertial navigation capability to a satellite navigation system. The use of a two axis accelerometer provides for precise adjustment for the direction of measurement of acceleration.
Several studies have also been conducted to estimate human body position and orientation using Kalman filters and extended Kalman filters (Yun and Bachmann (Reference Yun and Bachmann2006), Luinge, Veltink, and Baten (Reference Luinge, Veltink and Baten2007), Yun et al. (Reference Yun, Bachmann, Moore and Calusdian2007), Lee and Park (Reference Lee and Park2009) ). The use of the more recent UKF to human body relative position and orientation estimation is virtually non-existent. In this study, satellite based pseudo-range measurements are integrated with accelerometer measurements made by six accelerometers located on the six faces of a cuboid, to independently measure the translational and rotational accelerations, and the pseudo-range. These measurements are then processed by an UKF to correct for the estimated errors and to obtain the required position and translation velocities at two independent locations. The relative positions and velocities are obtained by the application of standard vector identities to two sets of independent measurements made at two locations. From these estimates, the position and velocity kinematics of prosthetic limbs and measurements of the joint angles, the true ambulatory position is estimated. The robotic limb joint offsets are assumed to be biased which are estimated by the UKF. The UKF is a feasible alternative to the linear and extended linear Kalman filters that have been proposed to overcome the difficulty of evaluating Jacobians of nonlinear state and process dynamic models, and is an effective way of applying the Kalman filter to nonlinear systems. The basic assumption is that the errors in the measurements are quite similar at the two locations and for this reason it is hypothesised that these errors would be reduced when the relative position and velocity were estimated.
2. GYRO-FREE SATELLITE AIDED INERTIAL POSITIONING
The concept of gyro-free measurement of angular acceleration using linear accelerometers was proposed by Schuler, Grammatikos and Fegley (Reference Schuler, Grammatikos and Fegley1967) almost forty years ago. Subsequently Padgoanker, Krieger and King (Reference Padgoanker, Krieger and King1975) and Mital and King (Reference Mital and King1979) considered the computation of rigid body rotations from measurements of linear acceleration obtained from body fixed linear accelerometers. Moreover it was felt that to obtain stable outputs of rotational motion a minimum of nine accelerometers were necessary. However Chen, Lee and DeBra (Reference Chen, Lee and DeBra1994) were able to show that six accelerometers are quite adequate to measure rigid body rotations. Since their work a few alternate schemes have emerged using nine accelerometers such as the one proposed by Wang, Ding and Zhao (Reference Wang, Ding and Zhao2003) . In most of these proposals the six accelerometer unit was considered as an independent sensor but was not fully integrated into a strapped down navigation system. Traditionally the computation of position and orientation in a strapped down navigation system has evolved as a two stage process; first the computation of the navigation position and orientation followed by the computation of the errors. Navigation measurements by an inertial measuring unit (IMU) must be calibrated and aligned, prior to being employed to provide estimates of position and orientation, relative to the North-East-Down navigation frame by the processes of levelling and gyro-compassing based on the direction of the gravity vector and earth rate sensing either while the vehicle remains stationary at a known location on the ground or in-flight. Furthermore most of these algorithms assume that only measurements of body accelerations and angular rates are available in three mutually perpendicular directions.
With availability of additional measurements a host of Kalman filter based fusion algorithms have been developed (see Adam, Rivlin and Rotstein (Reference Adam, Rivlin and Rotstein1999) for an example) to compensate for misalignment and IMU errors. The Kalman filter is itself a two stage process involving both state propagation and error correction. Kalman filter based approaches have been proposed to integrate imaging vision sensors to provide for multi-sensor inertial navigation and alignment (see for example Wang, Garratt, Lambert, Wang, Han, and Sinclair (Reference Wang, Garratt, Lambert, Wang, Han and Sinclair2008)) . One popular approach is to combine measurements made by a GPS receiver with the traditional strapped down navigation system measurements. When no rate gyro measurements are made and it is still to possible make other measurements using satellite navigation aids such as GPS, which can provide estimates of the pseudo-range or of carrier smoothed pseudo-range and the carrier phase differentials, so the algorithms for the computation of the navigation position and orientation can be greatly simplified.
GPS aided INS development has progressed in two distinct directions. First, there was been substantial effort to develop high fidelity navigation systems for attitude and position estimation. These include high accuracy systems for both geomatic and navigation applications. (see for example Qin, Zhang, Zhang and Xu (Reference Qin, Zhang, Zhang and Xu2006) ). In the main these systems have recommended the use of either highly sophisticated angular rate sensors or carrier phase and differential carrier phase measurement systems to achieve the improved accuracy. Second, for navigation applications Salychev, Voronov, Cannon, Nayak and Lachapelle (Reference Salychev, Voronov, Cannon, Nayak and Lachapelle2000) have considered the development of low cost GPS aid inertial navigation systems.
3. PROCESS MODELLING WITH GYRO-FREE ACCELERATION MEASUREMENTS
The basic navigation equations have been derived by Farrell and Barth (Reference Farrell and Barth1999) . These are summarised here for completeness:
![\eqalign{ \left[ {\matrix{ {\dot{V}_{N} } \cr {\dot{V}_{E} } \cr {\dot{V}_{D} } \cr} } \right] \equals \left[ {\matrix{ {A_{N} } \cr {A_{E} } \cr {A_{D} } \cr} } \right] \plus \left[ {\matrix{ 0 \cr 0 \cr g \cr} } \right] \plus \tab \left[ {\matrix{ { \minus \left( {2\omega _{s} \sin \lambda \plus {{V_{E} } \over {R_{P} \plus h}}\tan \lambda } \right)V_{E} \plus {{V_{N} V_{D} } \over {R_{M} \plus h}}} \cr {\left( {2\omega _{s} \sin \lambda \plus {{V_{E} } \over {R_{P} \plus h}}\tan \lambda } \right)V_{N} \plus \left( {2\omega _{s} \cos \lambda \plus {{V_{E} } \over {R_{P} \plus h}}} \right)V_{D} } \cr { \minus {{V_{N}^{\setnum{2}} } \over {R_{M} \plus h}} \minus \left( {2\omega _{s} \cos \lambda \plus {{V_{E} } \over {R_{P} \plus h}}} \right)V_{E} } \cr} } \right] \cr \tab \dot{\lambda } \equals {{V_{N} } \over {R_{M} \plus h}}\comma \quad \dot{\varphi } \equals {{V_{E} \sec \lambda } \over {R_{P} \plus h}}\comma \quad \dot{h} \equals \minus V_{D} \comma \cr}\comma\hskip-18pt](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171109-49789-mediumThumb-S0373463310000494_eqn1.jpg?pub-status=live)
where V N, V E and V D are the north, east and down velocities in the local tangent plane, with reference to a local geodetic frame often referred to as the navigation frame (n-frame) or north-east-down frame. The last three equations relate these velocities to the rate of change of the geodetic latitude (λ), the rate of change of longitude (ϕ) and the altitude (h) rate. A N, A E and A D are the north, east, down components of the measured acceleration in the n-frame which must be compensated by adding the acceleration due to gravity g, in down direction, ωs is angular velocity of the Earth, R M and R P are the radii of curvature in the meridian and prime vertical at a given latitude. Unit vectors in the n-frame are related to the unit vectors in the Earth centred inertial frame according to the relations,
![\eqalign{\left[ {\matrix{ {i_{G} } \cr {j_{G} } \cr {k_{G} } \cr} } \right] \equals {\bf D}_{n\comma I} \left[ {\matrix{ {i_{I} } \cr {j_{I} } \cr {k_{I} } \cr} } \right]\comma \quad {\bf D}_{n\comma I} \equiv \left[ {\matrix{ { \minus \sin \lambda \cos \left( {\varphi \plus \rmXi } \right)} \tab { \minus \sin \lambda \sin \left( {\varphi \plus \rmXi } \right)} \tab {\cos \lambda } \cr { \minus \sin \left( {\varphi \plus \rmXi } \right)} \tab {\cos \left( {\varphi \plus \rmXi } \right)} \tab 0 \cr { \minus \cos \lambda \cos \left( {\varphi \plus \rmXi } \right)} \tab { \minus \cos \lambda \sin \left( {\varphi \plus \rmXi } \right)} \tab { \minus \sin \lambda } \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171128-03078-mediumThumb-S0373463310000494_eqn2.jpg?pub-status=live)
where Ξ is the hour angle of the vernal equinox. The vector of the north, east, down components of the measured acceleration in the n-frame are related to the body components of the measured acceleration, by the transformation,
where the transformation of the measured body acceleration components to the north, east, down components in the n-frame Dn,b, satisfies the differential equation,
In equation 4 the matrix ΩG is obtained from the components of the angular velocity vector of the local geodetic frame or n frame. The angular velocity vector of the local geodetic frame or n frame may be expressed in terms of the Earth angular velocity in the local geodetic frame ωs as,
![\bmomega _{G} \equals \bmomega _{s} \plus \left[ {\matrix{ {\dot{\varphi }\cos \lambda } \cr { \minus \dot{\lambda }} \cr { \minus \dot{\varphi }\sin \lambda } \cr} } \right]{\rm \ with\ }\bmomega _{s} \equals \omega _{s} \left[ {\matrix{ {\cos \lambda } \cr 0 \cr { \minus \sin \lambda } \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn5.gif?pub-status=live)
Given a vector, ![\bmomega \equals \left[ {\matrix{ {\omega _{\setnum{1}} } \tab {\omega _{\setnum{2}} } \tab {\omega _{\setnum{3}} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU1.gif?pub-status=live){kind=small}
, ω× is defined by the relation,
![\bmomega _{ \times } \equals \left[ {\matrix{ 0 \tab { \minus \omega _{\setnum{3}} } \tab {\omega _{\setnum{2}} } \cr {\omega _{\setnum{3}} } \tab 0 \tab { \minus \omega _{\setnum{1}} } \cr { \minus \omega _{\setnum{2}} } \tab {\omega _{\setnum{1}} } \tab 0 \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn6.gif?pub-status=live)
Then ΩG is defined as ΩG=ωG×. Similarly Ωb is defined as Ωb=ωb× where ωb is the body angular velocity in the body fixed frame.
In principle, the scalar acceleration measurements may be expressed as,
![a_{\bf i} \equals {\bf z}^{i} \cdot \left[ {{\ddot{\bf R}}_{\bf I} \minus {\bf r}^{i} \times \dot{\bmomega } \plus \bmomega \times \bmomega \times {\bf r}^{i} } \right]\comma \ i \equals 1\comma \ 2\comma \ 3....6](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn7.gif?pub-status=live){kind=small}
where zi, is the direction of sensitivity of the i th accelerometer, ri the position vector of the accelerometer location in the body fixed frame, ω=ωb and 
is the inertial acceleration of the origin of the body frame. With six accelerometers it is, in principle, possible to express,
![\left[\!\! {\matrix{ { \minus {\bf z}^{i} \cdot {\bf r}_{ \times }^{i} } \tab {{\bf z}^{i} } \cr} } \right]\left[ {\matrix{ {\dot{\bmomega }} \cr {{\ddot{\bf R}}_{\bf I} } \cr} } \right] \equals a_{\bf i} \minus {\bf z}^{i} \cdot \left[ {\bmomega \times \bmomega \times {\bf r}^{i} } \right] \equals a_{i} \plus f_{i} \left( {{\bf z}^{\bf i} \comma {\bf r}^{\bf i} \comma \bmomega } \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn8.gif?pub-status=live)
where,
![f_{i} \left( {{\bf z}^{\bf i} \comma {\bf r}^{\bf i} \comma \bmomega } \right) \equiv \minus {\bf z}^{i} \cdot \left[ {\bmomega \times \bmomega \times {\bf r}^{i} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn9.gif?pub-status=live){kind=small}
It follows that,
Defining the vectors di as,
![\left[ {{\bf d}_{i} } \right] \equiv \left[\! {\matrix{ { \minus {\bf z}^{i} \cdot {\bf r}_{ \times }^{i} } \tab {{\bf z}^{i} } \cr} } \right] \equals \left[ {\matrix{ {z_{\setnum{3}}^{i} r_{\setnum{2}}^{i} \minus z_{\setnum{2}}^{i} r_{\setnum{3}}^{i} } \tab {z_{\setnum{1}}^{i} r_{\setnum{3}}^{i} \minus z_{\setnum{3}}^{i} r_{\setnum{1}}^{i} } \tab {z_{\setnum{2}}^{i} r_{\setnum{1}}^{i} \minus z_{\setnum{1}}^{i} r_{\setnum{2}}^{i} } \tab {z_{\setnum{1}}^{i} } \tab {z_{\setnum{2}}^{i} } \tab {z_{\setnum{3}}^{i} } \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn11.gif?pub-status=live){kind=small}
equation 8 may be expressed as,
![{\bf D}\left[ {\matrix{ {\dot{\bmomega }^{T} } \tab {{\ddot{\bf R}}_{\bf I}^{T} } \cr} } \right]^{T} \equals {\bf A} \plus {\bf F}\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn12.gif?pub-status=live){kind=small}
where, ![{\bf D} \equals \left[ {\matrix{ {{\bf d}_{\setnum{1}}^{T} } \tab {{\bf d}_{\setnum{2}}^{T} } \tab {{\bf d}_{\setnum{3}}^{T} } \tab {{\bf d}_{\setnum{4}}^{T} } \tab {{\bf d}_{\setnum{5}}^{T} } \tab {{\bf d}_{\setnum{6}}^{T} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU3.gif?pub-status=live){kind=small}
, ![{\bf A} \equals \left[ {\matrix{ {a_{\setnum{1}} } \tab {a_{\setnum{2}} } \tab {a_{\setnum{3}} } \tab {a_{\setnum{4}} } \tab {a_{\setnum{5}} } \tab {a_{\setnum{6}} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU4.gif?pub-status=live){kind=small}
, and ![{\bf F} \equals \left[ {\matrix{ {f_{\setnum{1}} } \tab {f_{\setnum{2}} } \tab {f_{\setnum{3}} } \tab {f_{\setnum{4}} } \tab {f_{\setnum{5}} } \tab {f_{\setnum{6}} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU5.gif?pub-status=live){kind=small}
.
Equation 12 may be expressed as,
![\dot{\bmomega } \equals \left[ {\matrix{ {{\bf I}_{\setnum{3} \times \setnum{3}} } \tab {{\bf 0}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A} \plus {\bf F}} \right)\comma \ {\ddot{\bf R}}_{\bf I} \equals \left[ {\left[ {\matrix{ {{\bf 0}_{\setnum{3} \times \setnum{3}} } \tab {{\bf I}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]} \right]{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A} \plus {\bf F}} \right).](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn13.gif?pub-status=live){kind=small}
At this stage it is important to recognise that the definition of the functions f i(zi, ri, ω), must be modified after considering that measurements of acceleration must be compensated by adding the local acceleration due to gravity. Furthermore the definition of the acceleration of gravity generally includes the centripetal acceleration due the earth's rotation rate vector, ωs. For this reason, one defines,
![\rmDelta f_{i} \left( {{\bf z}^{\bf i} \comma {\bf r}^{\bf i} \comma \bmomega } \right) \equiv {\bf z}^{i} \cdot \left( {\left[ {\bmomega _{s} \times \bmomega _{s} \times {\bf r}^{i} } \right] \minus \left[ {\bmomega \times \bmomega \times {\bf r}^{i} } \right]} \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn14.gif?pub-status=live){kind=small}
and the equations 13 may now be expressed as,
![\dot{\bmomega } \equals \left[ {\matrix{ {{\bf I}_{\setnum{3} \times \setnum{3}} } \tab {{\bf 0}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A}_{m} \plus {\bf G} \plus \rmDelta {\bf F} \plus {\bf b} \plus {\bf n}} \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn15.gif?pub-status=live){kind=small}
and the body components of the measured acceleration are,
![{\bf A}_{body} \equals \left[ {\left[ {\matrix{ {{\bf 0}_{\setnum{3} \times \setnum{3}} } \tab {{\bf I}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]} \right]{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A}_{m} \plus \rmDelta {\bf F} \plus {\bf b} \plus {\bf n}} \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn16.gif?pub-status=live){kind=small}
where, G is the gravitational component of the acceleration in the body frame, ![\rmDelta {\bf F} \equals \left[ {\matrix{ {\rmDelta f_{\setnum{1}} } \tab {\rmDelta f_{\setnum{2}} } \tab {\rmDelta f_{\setnum{3}} } \tab {\rmDelta f_{\setnum{4}} } \tab {\rmDelta f_{\setnum{5}} } \tab {\rmDelta f_{\setnum{6}} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU6.gif?pub-status=live){kind=small}
, b is a measurement bias vector and n is a measurement noise vector. It is possible to choose the location ri, and the direction of the measurements zi, such that, In equation 15:
![\left[ {\matrix{ {{\bf I}_{\setnum{3} \times \setnum{3}} } \tab {{\bf 0}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]{\bf D}^{ \minus {\bf \setnum{1}}} {\bf G} \equals {\bf 0}.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn17.gif?pub-status=live){kind=small}
Hence it follows that,
![\dot{\bmomega } \equals \left[ {\matrix{ {{\bf I}_{\setnum{3} \times \setnum{3}} } \tab {{\bf 0}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A}_{m} \plus \rmDelta {\bf F} \plus {\bf b} \plus {\bf n}} \right)\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn18.gif?pub-status=live){kind=small}
and
![{\bf A}_{NED} \equals {\bf D}_{n\comma b} \left[ {\left[ {\matrix{ {{\bf 0}_{\setnum{3} \times \setnum{3}} } \tab {{\bf I}_{{\bf \setnum{3}} \times {\bf \setnum{3}}} } \cr} } \right]} \right]{\bf D}^{ \minus {\bf \setnum{1}}} \left( {{\bf A}_{m} \plus \rmDelta {\bf F} \plus {\bf b} \plus {\bf n}} \right)\comma \ {\bf D}_{n\comma b} \equals {\bf D}_{n\comma I} {\bf D}_{b\comma I}^{ \minus \setnum{1}}.](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn19.gif?pub-status=live){kind=small}
When the accelerometers are located on the faces of a rectangular cuboid, as shown in Figure 1, the vectors zi and ri may be expressed as,
![\left[ {\matrix{ {{\bf z}^{\setnum{1}} } \cr {{\bf z}^{\setnum{2}} } \cr {{\bf z}^{\setnum{3}} } \cr {{\bf z}^{\setnum{4}} } \cr {{\bf z}^{\setnum{5}} } \cr {{\bf z}^{\setnum{6}} } \cr} } \right] \equals {1 \over {\sqrt 2 }}\left[ {\matrix{ 1 \tab 1 \tab 0 \cr 1 \tab 0 \tab 1 \cr 0 \tab 1 \tab 1 \cr 0 \tab { \minus 1} \tab 1 \cr { \minus 1} \tab 0 \tab 1 \cr { \minus 1} \tab 1 \tab 0 \cr} } \right]\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn20.gif?pub-status=live)
![\left[ {\matrix{ {{\bf r}^{\bf \setnum{1}} } \tab {{\bf r}^{\setnum{2}} } \tab {{\bf r}^{\setnum{3}} } \tab {{\bf r}^{\setnum{4}} } \tab {{\bf r}^{\setnum{5}} } \tab {{\bf r}^{\setnum{6}} } \cr} } \right] \equals \left[ {\matrix{ 0 \tab 0 \tab { \minus r_{\setnum{1}}^{\setnum{3}} } \tab {r_{\setnum{1}}^{\setnum{3}} } \tab 0 \tab 0 \cr 0 \tab { \minus r_{\setnum{2}}^{\setnum{2}} } \tab 0 \tab 0 \tab {r_{\setnum{2}}^{\setnum{2}} } \tab 0 \cr { \minus r_{\setnum{3}}^{\setnum{1}} } \tab 0 \tab 0 \tab 0 \tab 0 \tab {r_{\setnum{3}}^{\setnum{1}} } \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn21.gif?pub-status=live)
Equation 18 may now be integrated, in principle to obtain the body angular velocity vector, ω=ωb. The attitude quaternion is then computed from the equations,
![{\dot{\bf q}} \equals {{\bf 1} \over {\bf 2}}\ \bmOmega \left( \bmomega \right){\bf q}\comma \quad \bmOmega \left( \bmomega \right) \equals \left[ {\matrix{ 0 \tab {\omega _{\setnum{3}} } \tab { \minus \omega _{\setnum{2}} } \tab {\omega _{\setnum{1}} } \cr { \minus \omega _{\setnum{3}} } \tab 0 \tab {\omega _{\setnum{1}} } \tab {\omega _{\setnum{2}} } \cr {\omega _{\setnum{2}} } \tab { \minus \omega _{\setnum{1}} } \tab 0 \tab {\omega _{\setnum{3}} } \cr { \minus \omega _{\setnum{1}} } \tab { \minus \omega _{\setnum{2}} } \tab { \minus \omega _{\setnum{3}} } \tab 0 \cr} } \right]\comma \ \bmomega \equals \left[ {\matrix{ {\omega _{\setnum{1}} } \cr {\omega _{\setnum{2}} } \cr {\omega _{\setnum{3}} } \cr} } \right]\comma](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn22.gif?pub-status=live)
where the quaternion is subject to the constraint qT⋅q=1. Once the solution for the quaternion is known, the transformation from the inertial to the body fixed frame Db,I is computed from,
![{\bf D}_{b\comma I} \left( {\bf q} \right) \equals \left[ {\matrix{ {q_{\setnum{4}}^{\setnum{2}} \plus q_{\setnum{1}}^{\setnum{2}} \minus q_{\setnum{2}}^{\setnum{2}} \minus q_{\setnum{3}}^{\setnum{2}} } \tab {2\left( {q_{\setnum{1}} q_{\setnum{2}} \plus q_{\setnum{3}} q_{\setnum{4}} } \right)} \tab {2\left( {q_{\setnum{1}} q_{\setnum{3}} \minus q_{\setnum{2}} q_{\setnum{4}} } \right)} \cr {2\left( {q_{\setnum{1}} q_{\setnum{2}} \minus q_{\setnum{3}} q_{\setnum{4}} } \right)} \tab {q_{\setnum{4}}^{\setnum{2}} \minus q_{\setnum{1}}^{\setnum{2}} \plus q_{\setnum{2}}^{\setnum{2}} \minus q_{\setnum{3}}^{\setnum{2}} } \tab {2\left( {q_{\setnum{2}} q_{\setnum{3}} \plus q_{\setnum{1}} q_{\setnum{4}} } \right)} \cr {2\left( {q_{\setnum{1}} q_{\setnum{3}} \plus q_{\setnum{2}} q_{\setnum{4}} } \right)} \tab {2\left( {q_{\setnum{2}} q_{\setnum{3}} \minus q_{\setnum{1}} q_{\setnum{4}} } \right)} \tab {q_{\setnum{4}}^{\setnum{2}} \minus q_{\setnum{1}}^{\setnum{2}} \minus q_{\setnum{2}}^{\setnum{2}} \plus q_{\setnum{3}}^{\setnum{2}} } \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171134-00072-mediumThumb-S0373463310000494_eqn23.jpg?pub-status=live)
and its inverse is obtained by same equation by changing the sign of q 4. The required transformation Dn,b may then be computed without matrix inversion from Dn,IDb,I−1, the transformations from the inertial to the n-frame and the inverse transformation from the inertial to the body fixed frame. Alternately Dn,b may be computed directly from the associated quaternion, representing the relative attitude of the navigation from relative to the body frame.
The GYROCUBE: A sensor for inertial measurements; the directions of the arrows indicates the direction of sensitivity of the accelerometers.
4. SATELLITE NAVIGATION MEASUREMENTS MODELLING
Pseudo-range observations are obtained by measuring the time taken for the signal to propagate from the satellite to the receiver and multiplying the measurement by the speed of light after it has been corrected for bias errors. Due to the lack of complete synchronization between the receiver and satellite clocks, errors in the orbital ephemeris, the delays due to the ionosphere and troposphere and multipath effects the measured pseudo-range is always biased. The pseudo-range measurement for a single satellite may be expressed in terms of the speed of light c, the satellite clock delay error dt, the receiver clock error delay dT, as,
where, ρm is the measured pseudo-range, ρ is the true magnitude of the pseudo-range vector, δρe is the pseudo-range error due to errors in the orbital ephemeris, δρion is the pseudo-range error due to delays in the ionosphere, δρtrop is the pseudo-range error due to delays in the troposphere, δρp is the pseudo-range error due to multi-path effects and v is the measurement noise. The errors due to errors in the orbital ephemeris, satellite and receiver clock biases, delays in the ionosphere, delays in the troposphere, and those due to multi-path effects may be estimated. Thus the estimate of the pseudo-range may be expressed in terms of the actual magnitude of the pseudo-range vector ρ, as,
The actual pseudo-range vector is related to the geodetic latitude λ, geocentric latitude λs, longitude φ and altitude h, by the relations,
![\bmrho \equals \left[ {\matrix{ {r_{s} \cos \lambda _{s} \cos \phi \plus h\cos \lambda \cos \phi } \cr {r_{s} \cos \lambda _{s} \sin \phi \plus h\cos \lambda \sin \phi } \cr {r_{s} \sin \lambda _{s} \plus h\sin \lambda } \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn26.gif?pub-status=live)
where ρ is the Earth centred, Earth fixed position vector of the body, r s the radius at a surface point of the flattened Earth ellipsoid and λs are defined in terms of the flattening f and the equatorial radius R e as:
and
To complement these pseudo-range measurements we assume that we also have independent measurements of the altitude and east geodetic longitude. This is necessary as the altitude and longitude kinematics have been included in the process model. Measurements of the altitude may be obtained from a radar altimeter while there are a variety of ways to obtain the east geodetic longitude. Alternately the longitude kinematics may be deleted from the process model.
5. RELATIVE POSITION AND VELOCITY ESTIMATION
Given the position coordinates of two points located on two independent bodies A and B respectively in space and the orientation of the two bodies in terms of the individual body attitude quaternion the relative orientation of body B with respect to body A can, in principle, be obtained by simply reversing the quaternion of body B followed by a composition with the quaternion of body A. However such computations are generally unreliable and so the orientation is estimated from estimates of the relative position and velocity of the two bodies. The relative position can be determined by determining the relative position in the reference coordinates and transforming it to the base coordinates in the body A. The velocity vector of body B relative to body A is determined by transforming the velocity components of body B to the same frame used to define the velocity vector of body A and subtracting from them the velocity components of body A. From the relative position and velocity estimates and measurements of the joint angles an UKF type mixing filter is implemented to estimate the joint angles and the true relative position of the end-effector in an ambulatory condition.
6. THE ADAPTIVE UNSCENTED KALMAN FILTER
Most dynamic models employed for the purposes of estimation or filtering of pseudo range errors or orbit ephemeris errors are not linear. To extend and overcome the limitations of linear models, a number of approaches such as the EKF have been proposed in the literature for nonlinear estimation using a variety of approaches. Unlike the Kalman filter, the EKF may diverge, if the consecutive linearizations are not a good approximation of the linear model over the entire uncertainty domain. Yet the EKF provides a simple and practical approach to dealing with essential non-linear dynamics. The main difficulty in applying the algorithm to problems related to the estimation of a spacecraft's orbit and attitude is in determining the proper Jacobian matrices. The UKF is a feasible alternative that has been proposed to overcome this difficulty, by Julier, and Uhlmann (Reference Julier and Uhlmann2000) as an effective way of applying the Kalman filter to nonlinear systems. It is based on the intuitive concept that it is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function or transformation, of a random variable.
The UKF gets its name from the Unscented transformation, which is a method of calculating the mean and covariance of a random variable undergoing nonlinear transformation y=f(w). Although it is a derivative free approach, it does not really address the divergence problem. In essence the method constructs a set of sigma vectors and propagates them through the same non-linear function. The mean and covariance of the transformed vector are approximated as a weighted sum of the transformed sigma vectors and their covariance matrices.
Consider a random variable w with dimension L which is going through the nonlinear transformation, y=f(w). The initial conditions are that w has a mean 
and a covariance Pww. To calculate the statistics of y, a matrix χ of 2L+1 sigma vectors is formed as outlined by Julier, and Uhlmann (Reference Julier and Uhlmann2000) . In this work the scaled unscented transformation is used as this transformation gives one the added flexibility of scaling the sigma points to ensure that the transformed covariance matrices are always positive definite.
Given a general discrete nonlinear dynamic system in the form:
where xk∊R n is the state vector, uk∊R r is the known input vector, yk∊R m is the output vector at time k. wk and vk are, respectively, the disturbance or process noise and sensor noise vectors, which are assumed to Gaussian white noise with zero mean. Furthermore Qk and Rk are assumed to be the covariance matrices of the process noise sequence, wk and the measurement noise sequence, vk respectively. The unscented transformations of the states are denoted as:
while the transformed covariance matrices and cross-covariance are respectively denoted as,
and
The UKF estimator can then be expressed in a compact form. The state time-update equation, the propagated covariance, the Kalman gain, the state estimate and the updated covariance are respectively given by,
![{\bf \hat{x}}_{k} \equals {\bf \hat{x}}_{k}^{ \minus } \plus {\bf K}_{k} \left[ {{\bf z}_{k} \minus {\bf h}_{k}^{UT} \left( {{\bf \hat{x}}_{k}^{ \minus } } \right)} \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn32d.gif?pub-status=live){kind=small}
Equations 32 are in the same form as the traditional Kalman filter and the extended Kalman filter. Thus higher order non-linear models capturing significant aspects of the dynamics may be employed to ensure that the Kalman filter algorithm can be implemented to effectively estimate the states in practice.
In order to employ the UKF when precise statistics of the process and measurement noise vectors are not available, the adaptive filter method proposed by Song, Qi and Han, Reference Song, Qi and Han2006 is used to estimate the orbit parameters. The covariance matrixes of measurement residuals are recursively updated in the UKF. The measurement noise covariance matrices, in the case of the UKF, may be expressed as:
where, Crk,N is defined in terms of the sample size N and the residual rk as:
Equation 33 involves the further computation of 
khh, by applying the unscented nonlinear transformation, hkUT(○k) to the state estimate, ○k. The measurement noise covariance may be updated in principle by employing equation 33. The nonlinear relationships between the covariance matrices also suggests that the update of Rk could be done by employing the covariance of the residual.
In the application considered in this paper, the adaptation of Qk implemented as it is, the process statistics are often unknown or may be considered to vary. It was observed that the magnitudes of the filter gains were relatively small and for this reason the exact expression for an estimate of Qk:
was approximated as:
where CΔxk,N is defined as:
and
To update the process covariance matrix, Qk, the update law is assumed to be,
where the adaptation rate constant, αQ, is chosen to be a small number such as, αQ=0·01. It is also not required to update the process covariance matrix, Qk, after every integration time step. Thus the update of the process covariance matrix, Qk, is carried out only once every ten time steps, thus facilitating multi-rate adaptation.
7. APPLICATION TO A THREE LINK ROBOTIC LIMB
The reference frames and joint angles used to define the transformation of position and orientations, using the Denavit and Hartenberg convention, for a typical three-link robotic limb are shown in Figure 2. The link offsets and the link lengths are not shown on the same figure for clarity and are assumed to be h, d 1, d 2 and 0, a 1, a 2 respectively.
Reference frames and joint angles used to define the transformation of position and orientations for a typical three-link robotic limb.
The link offsets and the joint angles are estimates while the measurements of the joint angles are assumed to be available. The Denavit and Hartenberg transformations for relating the base frame to the end-effector frame are given by:
![\left[ \matrix{x_{\setnum{0}} \cr y_{\setnum{0}} \cr z_{\setnum{0}} \cr 1}\cr \right] \equals {\bf T}_{\setnum{0}\comma \setnum{1}} {\bf T}_{\setnum{1}\comma \setnum{2}} {\bf T}_{\setnum{2}\comma \setnum{3}} \left[\matrix{x_{e}\cr y_{e} \cr z_{e} \cr 1} \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn39.gif?pub-status=live)
![\openup2\eqalign{{\rm T}_{{\rm \setnum{0}\comma\! \setnum{3}}} \equals {\rm T}_{{\rm \setnum{0}\comma\! \setnum{1}}} {\rm T}_{{\rm \setnum{1}\comma\! \setnum{2}}} {\rm T}_{{\rm \setnum{2}\comma\! \setnum{3}}} \equals \tab \left[ {\matrix{ {\cos \theta _{\setnum{1}} } \tab 0 \tab {\sin \theta _{\setnum{1}} } \tab 0 \cr {\sin \theta _{\setnum{1}} } \tab 0 \tab {\minus \cos \theta _{\setnum{1}} } \tab 0 \cr 0 \tab 1 \tab 0 \tab h \cr 0 \tab 0 \tab 0 \tab 1 \cr} } \right] \left[ {\matrix{ {\cos \theta _{\setnum{2}} } \tab { \minus \sin \theta _{\setnum{2}} } \tab 0 \tab {a_{\setnum{1}} \cos \theta _{\setnum{2}} } \cr {\sin \theta _{\setnum{2}} } \tab {\cos \theta _{\setnum{2}} } \tab 0 \tab {a_{\setnum{1}} \sin \theta _{\setnum{2}} } \cr 0 \tab 0 \tab 1 \tab {d_{\setnum{1}} } \cr 0 \tab 0 \tab 0 \tab 1 \cr} } \right] \cr \tab \times \left[ {\matrix{ {\cos \theta _{\setnum{3}} } \tab { \minus \sin \theta _{\setnum{3}} } \tab 0 \tab {a_{\setnum{2}} \cos \theta _{\setnum{3}} } \cr {\sin \theta _{\setnum{3}} } \tab {\cos \theta _{\setnum{3}} } \tab 0 \tab {a_{\setnum{2}} \sin \theta _{\setnum{3}} } \cr 0 \tab 0 \tab 1 \tab {d_{\setnum{2}} } \cr 0 \tab 0 \tab 0 \tab 1 \cr} } \right] \cr}\hskip-15pt](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171208-65166-mediumThumb-S0373463310000494_eqn40.jpg?pub-status=live)
The velocity kinematics of the end-effector in the base coordinates may be expressed in terms of the joint angular velocity vector as;
![\left[ {\matrix{ {\dot{x}_{\setnum{00}} } \cr {\dot{y}_{\setnum{00}} } \cr {\dot{z}_{\setnum{00}} } \cr} } \right] \equals {\bf B}\left[ {\matrix{ {\dot{\theta }_{\setnum{1}} } \cr {\dot{\theta }_{\setnum{2}} } \cr {\dot{\theta }_{\setnum{3}} } \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn41.gif?pub-status=live)
where,
![\scale91%\eqalign{\tab B \equals\cr\tab \!\!\!\openup3\left[ {\matrix{ { \!\!\minus S\theta _{\setnum{1}} \left( {a_{\setnum{2}} C\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} C\theta _{\setnum{2}} } \right) \plus \left( {d_{\setnum{1}} \plus d_{\setnum{2}} } \right)C\theta _{\setnum{1}} }\!\!\! \tab { \minus C\theta _{\setnum{1}} \left( {a_{\setnum{2}} S\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} S\theta _{\setnum{2}} } \right)}\!\!\! \tab { \minus a_{\setnum{2}} C\theta _{\setnum{1}} S\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right)} \cr {C\theta _{\setnum{1}} \left( {a_{\setnum{2}} C\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} C\theta _{\setnum{2}} } \right) \plus \left( {d_{\setnum{1}} \plus d_{\setnum{2}} } \right)S\theta _{\setnum{1}} }\!\!\! \tab { \minus S\theta _{\setnum{1}} \left( {a_{\setnum{2}} S\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} S\theta _{\setnum{2}} } \right)}\!\!\! \tab { \minus a_{\setnum{2}} S\theta _{\setnum{1}} S\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right)} \cr 0 \tab {a_{\setnum{2}} C\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} C\theta _{\setnum{2}} } \tab {a_{\setnum{2}} C\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right)} \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171211-09516-mediumThumb-S0373463310000494_eqn42.jpg?pub-status=live)
Thus the joint angles may be expressed in terms of the translation velocities of the end-effector. In a practical situation, given noisy estimates of the relative velocities of the end-effector in the Cartesian base coordinates and noisy estimates of the end-effector relative position, the joint angular velocities may be estimated by solving a nonlinear filtering problem. Thus:
![\eqalign{\left[ {\matrix{ {\dot{\theta }_{\setnum{1}} } \cr {\dot{\theta }_{\setnum{2}} } \cr {\dot{\theta }_{\setnum{3}} } \cr} } \right] \equals {\bf B}^{ \minus {\bf \setnum{1}}} \left\{ {\left[ {\matrix{ {\dot{x}_{\setnum{00}m} } \cr {\dot{y}_{\setnum{00}m} } \cr {\dot{z}_{\setnum{00}m} } \cr} } \right] \plus \left[ {\matrix{ {w_{x} } \cr {w_{y} } \cr {w_{z} } \cr} } \right]} \right\}\comma \ \left[ {\matrix{ x_{\setnum{00}m} \cr y_{\setnum{00}m} \cr z_{\setnum{00}m}\cr 1}\cr} \right] \equals {\bf T}_{\setnum{0}\comma \setnum{3}} \left( {\theta _{\setnum{1}} \comma \theta _{\setnum{2}} \comma \theta _{\setnum{3}} } \right)\left[\matrix{ 0\cr 0\cr 0\cr 1} \right] \plus \left[ {\matrix{ {v_{x} } \cr {v_{y} } \cr {v_{z} } \cr 0 \cr} } \right]](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary-alt:20160626171208-72616-mediumThumb-S0373463310000494_eqn43.jpg?pub-status=live)
where ![\left[ {\matrix{ {w_{x} } \tab {w_{y} } \tab {w_{z} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU7.gif?pub-status=live){kind=small}
and ![\left[ {\matrix{ {v_{x} } \tab {v_{y} } \tab {v_{z} } \cr} } \right]^{T}](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqnU8.gif?pub-status=live){kind=small}
are vectors of independent white noise processes.
Equation 43 may be expressed as:
![\left[ {\matrix{ {x_{\setnum{00}m} } \cr {y_{\setnum{00}m} } \cr {z_{\setnum{00}m} } \cr} } \right] \equals {\bf H}\left( {\theta _{\setnum{1}} \comma \theta _{\setnum{2}} \comma \theta _{\setnum{3}} } \right) \plus \left[ {\matrix{ {v_{x} } \cr {v_{y} } \cr {v_{z} } \cr} } \right]\comma \ {\bf H} \equals \left[ {\matrix{ {C\theta _{\setnum{1}} \left( {a_{\setnum{2}} C\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} C\theta _{\setnum{2}} } \right) \plus \left( {d_{\setnum{1}} \plus d_{\setnum{2}} } \right)S\theta _{\setnum{1}} } \cr {S\theta _{\setnum{1}} \left( {a_{\setnum{2}} C\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} C\theta _{\setnum{2}} } \right) \minus \left( {d_{\setnum{1}} \plus d_{\setnum{2}} } \right)C\theta _{\setnum{1}} } \cr {a_{\setnum{2}} S\left( {\theta _{\rm \setnum{2}} \plus \theta _{\setnum{3}} } \right) \plus a_{\setnum{1}} S\theta _{\setnum{2}} \plus h} \cr} } \right].](https://static.cambridge.org/binary/version/id/urn:cambridge.org:id:binary:20151022041307601-0049:S0373463310000494_eqn44.gif?pub-status=live)
The joint angles are assumed to satisfy the bounds, −π<θ1<π,−π<θ2<π, −π<θ3<π. The matrix B is singular when the joint angle θ2=π/2. For this reason, when the joint angle θ2=π/2, the inverse of the matrix B is interpreted as the generalised inverse or pseudo inverse.
Given the position coordinates of the origin of reference frame at the end-effector, the link offsets and the link lengths, the joint angles may be computed in closed form. However, rather than use this closed form solution, a filtering problem is solved to obtain the true ambulatory position of the end effector. Thus it is assumed two satellite navigation aided inertial sensor packs are located at the origin of the base frame and at the origin of the end-effector. From the estimates of the position vector and velocity obtained from each location the joint angles and link offsets and biases may be estimated using a second independent UKF estimator to mix the position and velocity estimates at the two locations. Henceforth this second independent filter will be referred to as the inner filter while the two UKF estimators used to estimate the position and velocity at two locations are referred to as the outer filters.
In Figure 3 (Left) are shown comparisons of the relative position estimates produced by the outer filters, simulated and estimated end-effector relative position coordinates obtained by using the inner UKF based relative position estimator over a 6 second time frame corresponding to 30 000 uniform time steps. Initially the estimator seeks to track the simulated values but eventually tracks the outer filter estimates. Figure 3 (Right) shows comparisons of the corresponding simulated and estimated joint angle variations over the same time frame obtained by using three independent UKF based state estimators, without adaptation of the process covariance matrix, for the two satellite navigation aided accelerometer measurements and the robot kinematics position and velocity mixing. The figures indicate that the estimate is considerably noisy and that the filter tends to track the relative position estimates obtained by the outer filters in all three axes after about 20 seconds, rather than track the simulation. Thus the filter does not eliminate the drifts that may be present in the outer filter estimates.
(Left). Comparisons of the outer filter estimate, simulated and inner filter estimated end-effector position coordinates obtained by using UKF based state estimators over 30 000 time steps. (Right). Comparisons of the simulated and estimated joint angle variations over a 6 second time frame corresponding to 30 000 time steps.
In an attempt to reduce the noise in the estimate and improve the convergence of the outer filter estimate to the simulated relative position thus eliminating any drifts and biases that may be present, the adaptive UKF algorithm was used to estimate the positions and velocities at the two locations where the satellite navigation measurements are made, with the process covariance matrix updated recursively. Thus both the outer filters were configured to be adaptive while the inner mixing filter used to estimate the joint angles and link offsets continued to be non-adaptive. Figures 4 (Left) and (Right) illustrates the results obtained, with the process covariance matrices in the outer filters updated recursively, corresponding to Figures 3 (Left) and (Right) respectively. It can be easily observed that in the adaptive case, although the end-effector position estimated by the inner filter is substantially less noisy and that the estimate does latch on and track the true simulated relative position.
(Left) Comparisons of the outer filter estimate, simulated and inner filter estimated end-effector position coordinates obtained by using an adaptive UKF based position and velocity outer estimators over 30 000 time steps. (Right) Comparisons of the simulated and estimated joint angle variations corresponding to the adaptive UKF outer estimator over a 6 second time frame corresponding to 30 000 time steps.
The joint angles are now increased by a factor of three so they are all greater than π/2 over certain sections of the simulation time frame. The corresponding comparisons of the relative position estimates obtained from the outer filter estimates, simulated and estimated end-effector position coordinates obtained from the inner filter estimates are shown in Figure 5 (Left). In the outer filters the adaptive UKF estimator was used with the process covariance matrices updated recursively. The comparisons of the corresponding simulated and estimated joint angle variations over the same time frame are shown in Figure 5 (Right). The end-effector position coordinates continue to be estimated with almost the same accuracy as the case shown in Figure 4 (Left), although the joint angle estimates are relatively quite noisy.
(Left) Comparisons of the outer filter estimate, simulated and inner filter estimated end-effector position coordinates obtained by using an adaptive UKF based position and velocity outer estimators with the joint angles increased by a factor of three. (Right). Comparisons of the simulated and estimated joint angle variations corresponding to the adaptive UKF outer estimator, with the joint angles increased by a factor of three.
8. DISCUSSION AND CONCLUSIONS
In this paper the author has demonstrated the feasibility of implementing an adaptive unscented Kalman filter based mixing filter which may be used to estimate the relative position and orientations of prosthetic robotic limbs. The measurements were assumed to be made by six low cost ADXL203, type two axis accelerometers and a low cost altimeter. Measurements made by expensive sensors such as gyroscopes or high cost accelerometers, were assumed to be unavailable. The estimates of the relative position by using a standard UKF did not converge to the simulated relative position and the filters were unable to eliminate the drift in the position components. In an attempt to reduce the noise in the estimate and improve the convergence of the outer filter estimate to the simulated relative position thus eliminating any drifts and biases that may be present, the adaptive UKF algorithm was used to estimate the positions and velocities at the two locations where the satellite navigation measurements are made, with the process covariance matrix updated recursively. Thus both the outer filters were configured to be adaptive while the inner mixing filter used to estimate the joint angles and link offsets continued to be non-adaptive. When the adaptive UKF algorithm was used in the outer estimators there was substantial reduction in the errors in the estimated user position at the end of time frame while the filter itself converged to a steady state faster than the non-adaptive implementation. Thus the integrity of the ambulatory position estimated was maintained as the filter was able to eliminate all drifts and biases introduced by the outer filters. For this reason the adaptive filter was used to estimate the steady state ambulatory position of the end-effector after a disturbance or motion initiation. The steady-state ambulatory position error, in the relative position of the end-effector obtained from the outer filter estimates, is reduced by more than 98%. A major portion of the error in the relative position obtained from the outer filter estimates is a steady drift which is completely eliminated.
