2. KINEMATICS OF JERK AND JOUNCE

As stated before, in this work we will utilise the first and second derivatives of acceleration (jerk and jounce) to improve the positioning accuracy of a quad-copter. The elementary concepts on kinematics of jerk and jounce is attainable in Visser (Reference Visser2004) and Gibbs et al. (Reference Gibbs and Gragert1998). The first derivative of acceleration is known with terms jerk, jolt, surge, and lurch, of which the first one is in common use. Jerk is the temporal rate of change in the amount of acceleration that is incarnated as its time-derivative. It can be calculated by the second and third derivation of velocity and position, respectively as noted below:

(1) $$\overrightarrow {\rm j} ({\rm t}) = \displaystyle{{{\rm d}\overrightarrow {\rm a} ({\rm t})} \over {{\rm dt}}} = \overrightarrow {\mathop {\rm a}\limits^. } ({\rm t}) = \displaystyle{{{\rm d}^2\overrightarrow {\rm v} ({\rm t})} \over {{\rm d}{\rm t}^2}} = \overrightarrow {\mathop {\rm v}\limits^{..} } ({\rm t}) = \displaystyle{{{\rm d}^3\overrightarrow {\rm r} ({\rm t})} \over {{\rm d}{\rm t}^3}} = \overrightarrow {\mathop {\rm r}\limits^{...}} ({\rm t})$$

The fourth time-derivative of position and first, second and third time derivatives of, respectively, jerk, acceleration, and velocity is called jounce or snap. In other words, jounce is time-derivative of jerk and adheres to the following equation:

(2) $$\vec{\hbox{s}}=\displaystyle{{\hbox{d}\vec{\hbox{j}}}\over{\hbox{dt}}}= \displaystyle{{\hbox{d}^{2}\vec{\hbox{a}}}\over{\hbox{dt}^{2}}}=\displaystyle{{\hbox{d}^{3} \vec{\hbox{v}}}\over{{\hbox{dt}}^{3}}}=\displaystyle{{\hbox{d}^{4}\vec{\hbox{r}}}\over{{\hbox{dt}}^{4}}}$$

The following equations describe a constant-jounce kinematic problem, which is utilised in this work:

(3) $$\vec{j}=\vec{j}_{0}+\vec{s}t$$

(4) $$\vec{a}=\vec{a}_{0}+\vec{j}_{0}t+\displaystyle{{1}\over{2}}\,\vec{s}t^{2}$$

(5) $$\vec{v}=\vec{v}_{0}+\vec{a}_{0}t+\displaystyle{{1}\over{2}}\,\vec{j}_{0}t^{2}+\displaystyle{{1}\over{6}}\,\vec{s}t^{3}$$

(6) $$\vec{r}=\vec{r}_{0}+\vec{v}_{0}t+\displaystyle{{1}\over{2}}\,\vec{a}_{0}t^{2}+\displaystyle{{1}\over{6}}\,\vec{j_{0}}t^{3}+\displaystyle{{1}\over{24}}\,\vec{s}t^{4}$$

where $\vec{s}$ is the constant-jounce term, $\vec{j}_{0}$ initial jerk, $\vec{j}$ final jerk, $\vec{a}_{0}$ initial acceleration, $\vec{a}$ final acceleration, $\vec{v}_{0}$ initial velocity, $\vec{v}$ final velocity, $\vec{r}_{0}$ initial position, $\vec{r}$ final position, and t the specified time. The reader should not confuse notation $\vec{s}$ with the displacement vector in other texts. By applying the first and second derivative in the Kalman filter, we achieve Figure 1. As seen, jerk and jounce augment the state-space vector in the first stage of estimation loop (prediction) and are distributed to the other stages through Jacobean and Covariance matrices.

Figure 1.

Implementation of the first and second derivatives of acceleration on KF.

3. MODELLING AND MECHANISATION OF THE SINS

Because, in this work, SINS is exploited as the main navigational tool, a detailed description is given here. An INS is a self-dependent navigation method in which relative position and direction of motion is measured using data provided by an accelerometer and gyroscope. This method utilises the Dead-Reckoning (DR) principle, which helps to estimate current position of a vehicle from the history of position, accelerations and angular velocities of inertial sensors. An Inertial Measuring Unit (IMU) usually consists of three gyroscopes and three orthogonal accelerometers. Angular rates are used for calculating platform attitude via pitch, roll and yaw and transferring navigational parameters from the body frame to the navigation frame. The function of an INS is based on classical Newtonian mechanics laws. Specific acceleration is the output of an accelerometer, and three orthogonal accelerometers are usually present in an IMU (each directed toward a coordination axis). An INS can be considered as three main units: an Inertial Measurement Unit (IMU), a pre-processing unit, and a mechanisation module. As stated, in our effort, noise and perturbations are omitted from signals by implementation of a pre-processing filter. These signals are then fed into mechanising algorithms to find position and attitude parameters.

An on board mechanism makes it possible to determine state variables (i.e. position, velocity, acceleration) from initial values (accelerations and angular velocities) through solving differential equations describing system dynamics. Imperfect inertial data are measured in the Body frame which are relative to the Earth-Centred-Inertial (ECI) frame. Each method for SINS is described by a system of differential equations in the navigation frame (standard NED (North-East-Down)). Differential equations of SINS (Reinstein, Reference Reinstein2010) are described as follows:

(7) $$\dot{\hbox{v}}_{{\rm n}}=\hbox{C}_{{\rm b}}^{{\rm n}}\hbox{a}^{{\rm b}} + g^{{\rm n}}-\lpar \hbox{2}{\rm \omega}_{{\rm ie}}^{{\rm n}}+{\rm \omega}_{{\rm en}}^{{\rm n}} \rpar \times \hbox{v}^{{\rm n}}$$

(8) $$\dot{\hbox{C}}_{{\rm n}}^{{\rm e}}=\hbox{C}_{{\rm n}}^{{\rm e}} \lpar {\rm \omega}_{{\rm en}}^{{\rm n}}\times\rpar $$

(9) $$\dot{h}=-\hbox{v}_{{\rm D}}$$

(10) $$\dot{\hbox{C}}_{{\rm b}}^{{\rm n}}=\hbox{C}_{{\rm b}}^{{\rm n}}\lpar {\rm \omega}_{{\rm ib}}^{{\rm b}}\times \rpar -\lpar {\rm \omega}_{{\rm in}}^{{\rm n}}\times \rpar \hbox{C}_{{\rm b}}^{{\rm n}}$$

where

(11) $${\rm \omega}_{{\rm in}}^{{\rm n}}={\rm \omega}_{{\rm ie}}^{{\rm n}} + {\rm \omega}_{{\rm en}}^{{\rm n}}$$

in which subscripts and superscripts b, n, e and i denote the Body, Navigation, Earth-Centred-Earth-Fixed (ECEF), and ECI frames, respectively, and $C_{y}^{x}$ refers to transformation from y to x frames. Figure 2 presents the related classic diagram of the SINS. Appropriate selection of coordinate frames and transformation between them are essential in developing navigation algorithms. As previously hinted, inertial sensors measure movement related to an inertial frame, while strapdown navigational values are expressed in the body frame, and then transformed into the navigation frame. To transform measurements between the frames efficiently, we need to have sufficient information about them. In this work, the ECEF/e-frame is used for the vehicle's position mechanisation. Moreover, the vehicle's Euler angles are expressed in the navigation frame (NED). This frame is used to show attitude and velocity of the robot on the earth or in its vicinity. Its origin coincides with the Navigation frame and its x, y and z-axes are respectively pointed to the north, east and down directions. In the current work, by means of strapdown systems, experimental data (accelerations and angular velocities) are measured in the body frame where it is defined to lie on sensitivity axes of sensors and adheres to the right hand rule.

Figure 2.

Classic diagram of SINS.

As mentioned in Grewal et al. (Reference Grewal, Weill and Andrews2007), the KF is based on the supposition that type, number and characteristics of sensor noise is determined statistically and combined optimally. Based on this concept, some general restrictions on picking out the sensors in the current work are as follows. Frequency response of the sensor must be compatible with signals initiated by the state variable. Then sensor noise spectrum must be separable from the spectrum-state system. In addition, sensors are chosen regarding the observability of the system; i.e. the system states must be determinable and detectable by magnitudes. Covariance analysis of the KF and observability analysis is done for each combination of sensors for optimising the performance of the KF. Moreover, errors in accelerometers and gyroscopes results in a steady increment of error in components of position and velocity of the flying vehicle, due to integration. Therefore, the utilised SINS should be augmented by an external source to ameliorate the problem of the errors which disturb navigation solutions, such as bias, drift, scale factor deviations and external disturbances. These errors are a major concern in the navigation process and make correct calibration a necessity. In addition, in the current work, calibration of the sensors is performed manually and without any technical equipment.

4. ESTIMATION AND FILTERING PROCESS

The accuracy of SINS is always affected by inertial sensor errors. To decrease these errors, sensor data are passed through the system's filter just before initiation of navigation calculations. Depending on mission needs, different filters can be used. In this section, the implemented filters in this work are introduced.

4.1. The Complementary Filter (CF)

It is possible to find angular positions of a system using information from either accelerometers or gyroscopes. Nevertheless, both sensors suffer from considerable deficiencies, which necessitates incorporating suitable filters. CF is one of the most primitive methods for fusing information from accelerometers and gyroscopes. This filter utilises data from the gyroscope in the short term, thanks to its premium accuracy and insensitivity to external forces. On the other hand, accelerometer data are eligible for long term errors and do not deviate considerably, which makes them suitable for the filter. CF is composed of a low-pass filter to attenuate short term oscillations of accelerometers beside a high-pass filter to compensate for angular deviations of gyroscopes. Here, just a low-pass filter is used for data measured by the accelerometers (Lacanette, Reference Lacanette1991). In the following, we will describe implementation of the utilised discrete low-pass filter. A large number of digital filters are designed to show low-pass properties. Responses of both bounded and unbounded impulses from low-pass and Fourier transformation filters are very common. By regarding Figure 3 and observing Kirchhoff Laws, an electrical circuit including capacitors is defined as below (Cartwright et al., Reference Cartwright, Russell and Kaminsky2012).

Figure 3.

Electrical circuit for a low-pass filter.

(12) $$v_{in}(t)-v_{out}(t)=RC \displaystyle{{{dv}_{out}}\over{dt}}$$

This equation can be discretized. For simplification, it is supposed that internal and external samples are located at points at the same time intervals Δ T. The samples in v _in are $\lpar x_{1}\comma \; x_{2}\comma \; \ldots\comma \; x_{n}\rpar $ while in v _out are $\lpar y_{1}\comma \; y_{2}\comma \; \ldots\comma \; y_{n}\rpar $ , which is in correspondence to temporal points. By substitution, this yields:

(13) $$x_{i}-y_{i} = {\it {RC}}\ \displaystyle{{y_{i}-y_{i-1}}\over{\Delta T}}$$

Then rearrangement produces a recursive relation as follows:

(14) $$y_{i}=x_{i}\left( \displaystyle{{\Delta T}\over{RC+\Delta T}} \right)+y_{i-1}\left( \displaystyle{{RC}\over{RC+\Delta T}} \right)$$

in which the first term is input and the second one is output at the previous step. This formula is a discrete-time implementation of a simple Resistor–Capacitor (RC) low-pass filter for the exponentially weighted moving average circumstances:

(15) $$y_{i}=\propto x_{i}+\lpar 1-\propto \rpar y_{i-1}$$

in which:

(16) $$\propto =\displaystyle{{\Delta T}\over{RC+\Delta T}}$$

This is defined based on a smoothing factor: $0\le \propto \le 1$ . Factor ∝ results in equivalency of time constant RC in terms of sampling periods Δ T and smoothing factor ∝ itself:

(17) $$RC=\Delta T\left( \displaystyle{{1-\propto }\over{\propto}} \right)$$

It needs to be noted that:

(18) $$f_{C}=\displaystyle{{1}\over{2\pi RC}}$$

where it is the cut-off frequency and therefore we have:

(19) $$RC=\displaystyle{{1}\over{2\pi f_{c}}}$$

Then, f _c and ∝ are related through the following equation:

(20) $$\propto =\displaystyle{{2\pi \Delta_{T}f_{c}}\over{2\pi \Delta_{T}f_{c}+1}}$$

which gives:

(21) $$f_{c}=\displaystyle{{\propto }\over{( 1-\propto)2\pi \Delta T}}$$

For ∝ = 0.5, time constant Δ T is equal to the sampling period. When $\propto \ll 0.5$ , RC is greater than sampling period and we have $\Delta T\approx \propto R$ .

4.2. The KF

Regarding the preceding discussions, a SINS is affected by sensor errors, installation, assembling method and computational algorithms. This problem is worsened in low-cost MEMS sensors that are easily deviated. Due to multiple stochastic errors, these sensors show a poor performance when used independently. Main errors include accelerometer and gyroscope deviations. To improve accuracy of an INS, uniform time intervals K need to be chosen in a manner such that stochastic errors of the system can be estimated as biases. KF is an estimation algorithm to find optimum state errors of a system infected by noise. This filter is an ordered-recursive algorithm, which provides an optimum estimation of minimum variance of average from state errors. In addition, a KF produces real-time statistical data by estimating error states that are applicable in a quantitative error analysis (Wei and Wang, Reference Wei and Wang2013; Dove and Miller, Reference Dove and Miller1989).

4.2.1. Discrete-time KF

A discrete-time linear system is defined as below (Noureldin et al., Reference Noureldin, Karamat and Georgy2012; Dove and Miller, Reference Dove and Miller1989):

(22) $$\hbox{x}_{{\rm k}}={\Phi }_{{\rm k},{\rm k}-1}\hbox{x}_{{\rm k}-1}+\hbox{G}_{{\rm k}-1}\hbox{w}_{{{\rm k}-1}}$$

in which, x _k , Φ _{k, k−1}, G _k−1, w _k−1, and k are state vector, State Transformation Matrix (STM), error distribution matrix, process noise vector, and the step number, respectively. While, noise source may affect more than one component of the system state vector, a distribution matrix of noises G is introduced which can show the presence of noise in several components of the state variables. Linear equations for system measurements in a discrete time domain are as follows:

(23) $$\hbox{z}_{{\rm k}}=\hbox{H}_{{\rm k}}\hbox{x}_{{\rm k}}+\eta_{{\rm k}}$$

in which z _k is the system's output vector, H_k design or observation matrix and η_k process noise. As seen in Figure 4, setting up a filter involves two general tasks: (1) prediction or time update, and (2) correction or measurement update. In the prediction section, the system model is applied to propagate both current state of system and the related covariance estimation through k to k − 1 stages. Then in the correction phase, measurements are used for updating the prior predictions.

Figure 4.

Combination of Kalman filter, jerk, and jounce with measurement model and the discrete process.

4.2.2. Time update or prediction

Estimating a system state x in time k by only information from time k − 1 is called $\hat{\hbox{x}}_{k}\lpar -\rpar $ . While system noise due to the assumptions of Gaussian distribution has zero mean value, the best prediction of system state at time k is as noted below:

(24) $$\hat{\hbox{x}}_{{\rm k}}(-)=\Phi_{{\rm k}\,\vert\, {\rm k}-1}\ \hat{\hbox{x}}_{{\rm k}-1}(+)$$

$\hbox{x}_{{\rm k-1}}\lpar +\rpar $ is the optimum state estimate in the previous step which is called a prior estimate and is constructed based on the process model presented by $\Phi_{{ k}\, \vert\, {k}\hbox{-}1}$ STM. Kalman filter also propagates uncertainties of its estimation from step k-1 to k in form of covariance matrix $\hbox{P}_{\rm k}\lpar -\rpar $ that is also called a prior covariance matrix:

(25) $$\hbox{P}_{{\rm k}}(-)=\Phi_{{\rm k}\,\vert\, {\rm k}-1}\hbox{P}_{{\rm k}-1}(+) \Phi^{{\rm T}}_{{\rm k}\, \vert \,{\rm k}-1}+\hbox{G}_{{\rm k}-1} \hbox{Q}_{{\rm k}-1}\hbox{G}^{{\rm T}}_{{\rm k-1}}$$

where, $P_{{\rm k}-1}\lpar +\rpar $ is the optimum estimate of covariance in the precedent step, which is a corrected (posteriori) estimate of covariance matrix; based on the measurement in step k − 1. It is noted that a predicted (priori) covariance matrix $\hbox{P}_{{\rm k}}\lpar -\rpar $ depends on both process noise and posteriori covariance matrix ${\hbox{P}}_{{\rm k}-1}\lpar +\rpar $ .

4.2.3. Measurement Update (Correction)

After the state has been predicted by KF, the estimation is corrected by a measurement provided by external sources. Firstly, based on R_k measurement covariance, a weighing factor named Kalman gain is calculated so the Root Mean Square (RMS) error obtains a minimum value:

(26) $$\hbox{K}_{{\rm k}}=\hbox{P}_{{\rm k}}(-)\hbox{H}_{{\rm k}}^{{\rm T}}{[\hbox{H}_{{\rm k}}\hbox{P}_{{\rm k}} (-)\hbox{H}_{{\rm k}}^{{\rm T}}+\hbox{R}_{{\rm k}}]}^{{-1}}$$

In fact, KF is distinguished from other algorithms by this gain. As seen in Equation (26), K depends on both former covariance $\hbox{P}_{{\rm k}}\lpar -\rpar $ and measurement noise covariance. For measurements affected by noise (as R_k increases) or when process noise is smaller (for decrement of ${\hbox{P}}_{{\rm k}}\lpar -\rpar $ ), Kalman gain is augmented or shrunk, respectively. When a new measurement of Z_k was obtained at time ${\hbox{t}}_{{\rm k}}$ , it is compared to predicted measurement $\hbox{H}_{{\rm k}}\hat{\hbox{x}}_{{\rm k}}\lpar -\rpar $ based on a former state estimate. K weights their difference and prediction-update of the state vector is done to have an optimal estimate. Therefore, estimate of state in time t_k is

(27) $$\hat{\hbox{x}}_{{\rm k}}(+)=\hat{\hbox{x}}_{{\rm k}}(-)+\hbox{K}_{{\rm k}}{\rm [}\hbox{z}_{{\rm k}}-\hbox{H}_{{\rm k}}\hat{\hbox{x}}_{{\rm k}}(-){\rm ]}$$

in which $\hbox{H}_{{\rm k}}\hat{\hbox{x}}_{{\rm k}}{\lpar -\rpar }$ is the predicted observation represented by $\hat{\hbox{x}}_{k}$ , while $\hbox{z}_{{\rm k}}-\hbox{H}_{{\rm k}}\hat{\hbox{x}}_{{\rm k}}\lpar -\rpar $ is called the innovation sequence. Based on K, Kalman filter updates uncertainty of the new prediction $\hat{\hbox{x}}_{k}\lpar +\rpar $ is as below:

(28) $$\hbox{P}_{{\rm k}}(+)=[\hbox{I}-\hbox{K}_{{\rm k}}\hbox{H}_{{\rm k}}]\hbox{P}_{{\rm k}}(-)$$

5. LOOSELY COUPLED SINS/GNSS INTEGRATION

GPS and INS each have several pros and cons. INS is a self-dependent navigation system, which is usually utilised in the band of 200–1000 HZ in aerial missions; however, in some applications it may be used at a lower frequency than 200 HZ. It has an acceptable short-term accuracy and produces information on attitude (in addition to position and velocity). However, over the long term, INS errors accumulate due to the integration of measurements to obtain velocity and position. In contrary to INS, GNSS has a good long-term accuracy (with errors in the order of a few metres) and is supported by low-cost hardware. Meanwhile, its short-term accuracy and rate of output information is lower than INS. Originally, GNSS receivers did not provide attitude information, but using auxiliary hardware and software, this can be achieved. To employ a GNSS receiver, at least four satellites must be in direct line of sight, which is interrupted in some situations like being obscured by objects (e.g. constructions, trees, tunnels) or signal attenuation in atmospheric layers (Grewal et al., Reference Grewal, Weill and Andrews2007). It is worth mentioning that the atmosphere introduces large errors to GNSS computations, but it does not affect the line-of-sight situation. By using complementary properties of these two systems, their cooperation covers defects of each one and provides an accurate and robust navigation solution. To exploit complementary properties of two systems, a variety of structures are conceivable to fuse them for highest effectiveness, simplicity of usage and robustness of which loosely coupled, tightly coupled and ultra-tightly (deeply) coupled structures are the most important (Noureldin et al., Reference Noureldin, Karamat and Georgy2012). In this paper, a loosely coupled structure is preferred in which INS and GNSS act independently and provide separate navigation solutions. The INS solution is applied to a filter, where the filter determines the difference between two solutions and estimates INS error based on error function. The INS solution is corrected using this error to yield total navigation solutions for position, velocity and attitude. The distinctive feature of this configuration is exploitation of a separate filter for GNSS. A block diagram of the loosely coupled INS/GNSS integration is given in Figure 5. It is pointed out that, in this work, two distinct KFs are operated successively; the first one is implemented in attitude estimation, and the second one updates position, velocity, jerk and jounce state variables.

Figure 5.

Block diagram of loosely coupled SINS/GNSS data fusion.

5.1. Attitude Estimation

To enhance robustness of the attitude estimation process, both magnetometers and accelerometers are used in the observation of the Classical Euler angles. The process model in attitude estimation, which is used for prediction of roll, pitch, and yaw angles, exploits angular rates as input variables in the Body frame, and updates the former estimation of the Euler angles. The mathematical model for prediction of current attitude states is considered as follows:

(29) $$\eqalign{\left[{\matrix{ \varphi \cr \theta \cr \psi \cr}}\right]_{k} & =\left[{\matrix{ 1 & 0 & 0 \cr 0 & 1 & 0 \cr 0 & 0 & 1\cr}}\right] \left[{\matrix{\varphi \cr \theta \cr \psi \cr}}\right]_{k-1} +\left[{\matrix{ dt & sin\varphi tan\theta dt & cos\varphi tan\theta dt \cr 0 & cos\varphi dt & -sin\varphi dt \cr 0 & \displaystyle{{sin\varphi dt}\over{cos\theta}} & \displaystyle{{cos\varphi dt}\over{cos\theta}}\cr}}\right]_{k-1} \left[{\matrix{p \cr q \cr r \cr}}\right]_{k-1} \cr & \quad + w_{k}, w_{k}\sim N(0,Q_{k})}$$

in which $ \varphi, \theta, \psi $ are Euler angles, p, q, r are angular rates from gyroscopes and w _k is the process noise. In the following lines, the mathematical model in the observation process of Euler angles, based on magnetometer and accelerometer measurements is presented. Concisely, the overall format and notations of the model are similar to Equation (23). Hence, the state variables and the observation matrix are considered as follows:

(30) $$X = [\varphi, \theta, \psi]$$

(31) $$H=\left[{\matrix{ 1 & 0 & 0 \cr 0 & 1 & 0\cr 0 & 0 & 1\cr}}\right]$$

Furthermore, the observation vector and the related elements are considered as noted below:

(32-1) $$Z=[\matrix{\varphi_{acc} & \theta_{acc} & \psi_{mag}\cr}]$$

(32-2) $$\varphi_{acc}=-arcsin\left(\displaystyle{{a_{y}}\over{{(a_{x}^{{\rm 2}}+a_{y}^{{\rm 2}}+a_{z}^{{\rm 2}})}^{{\rm 0.5}}}}\right)$$

(32-3) $$\theta_{acc}=arcsin\left(\displaystyle{{a_{x}}\over{{(a_{x}^{{\rm 2}}+a_{y}^{{\rm 2}}+a_{z}^{{\rm 2}})}^{{\rm 0.5}}}}\right)$$

(32-4) $$\psi_{mag}=-arctan\lpar m_{y}/m_{x}\rpar $$

where a _x , a _y , a _z are components of accelerometer measurements, m _x , m _y are elements of magnetometer measurements and $\varphi _{acc}\comma \; \theta_{acc} \comma \; \psi_{mag}$ are roll, pitch, and yaw angles in the observation vector, respectively.

5.2. Constant Acceleration Process Model

The state vector of the process dynamic model, which is used for loosely coupled fusion method and utilised in the absence of the first and second derivatives of acceleration, is brought as follows. It constitutes Position, Velocity and Attitude (PVA) components along coordination axes.

(33) $$X=[\matrix{x & y & z & {\dot x} & {\dot y} & {\dot z}& \ddot{x}& \ddot{y}&\ddot{z} \cr}]^T$$

Dynamic coefficient matrix Φ without first and second derivative theories is as below

(34) $$\Phi=\left[{\matrix{1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0\cr 0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0\cr 0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}\cr 0&0&0&1&0&0& dt&0&0\cr 0&0&0&0&1&0&0& dt&0\cr 0&0&0&0&0&1&0&0& dt\cr 0&0&0&0&0&0&1&0&0\cr 0&0&0&0&0&0&0&1&0\cr 0&0&0&0&0&0&0&0&1\cr}}\right]$$

5.3. Constant Jounce Process Model

For attaining the goal of this research, the state vector of the Kalman filter is defined as below in which third and fourth derivatives of position are jerk and jounce which are treated as state variables.

(35) $$X=[\matrix{x & y & z& \dot{x} & \dot{y}& \dot{z}& \ddot{x}& \ddot{y}& \ddot{z}& {\mathop{x}\limits^{\ldots}}& {\mathop{y}\limits^{\ldots}}& {\mathop{z}\limits^{\ldots}}& x^{(4)}& y^{(4)}& z^{(4)}\cr}]^T$$

The system transmission matrix is shown in the following equation:

(36) $$\Phi=\left[{\matrix{1& 0& 0& dt& 0& 0& \displaystyle{{dt^2}\over{2}}& 0& 0& \displaystyle{{dt^3}\over{6}}& 0& 0& \displaystyle{{dt^4}\over{24}}& 0& 0\cr 0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0&\displaystyle{{dt^3}\over{6}}&0&0&\displaystyle{{dt^4}\over{24}}& 0 \cr 0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0&\displaystyle{{dt^3}\over{6}}&0&0&\displaystyle{{dt^4}\over{24}}\cr 0&0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0&\displaystyle{{dt^3}\over{6}}&0&0\cr 0&0&0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0&\displaystyle{{dt^3}\over{6}}&0\cr 0&0&0&0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0&\displaystyle{{dt^3}\over{6}}\cr 0&0&0&0&0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0&0 \cr 0&0&0&0&0&0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}&0\cr 0&0&0&0&0&0&0&0&1&0&0& dt&0&0&\displaystyle{{dt^2}\over{2}}\cr 0&0&0&0&0&0&0&0&0&1&0&0& dt&0&0\cr 0&0&0&0&0&0&0&0&0&0&1&0&0& dt&0\cr 0&0&0&0&0&0&0&0&0&0&0&1&0&0& dt\cr 0&0&0&0&0&0&0&0&0&0&0&0&1&0&0\cr 0&0&0&0&0&0&0&0&0&0&0&0&0&1&0\cr 0&0&0&0&0&0&0&0&0&0&0&0&0&0&1\cr}}\right]$$

5.4. Observation Model

For both of the abovementioned cases, the observation model is the same, where the observation vector is proposed as:

(37) $$Z=[\matrix{x_{gps}& y_{gps}& z_{gps}& a_x& a_y& a_z\cr}]^T$$

In addition, the observation design matrix H is in a form as noted below:

(38) $$H=\left[{\matrix{1&0&0&0&0&0&0&0&0&0&0&0&0&0&0\cr 0&1&0&0&0&0&0&0&0&0&0&0&0&0&0\cr 0&0&1&0&0&0&0&0&0&0&0&0&0&0&0\cr 0&0&0&0&0&0&0&0&0&0&0&0&1&0&0\cr 0&0&0&0&0&0&0&0&0&0&0&0&0&1&0\cr 0&0&0&0&0&0&0&0&0&0&0&0&0&0&1\cr}}\right]$$

In the following section, test results and the related practical aspects of the work are discussed.

6. TEST RESULTS

As mentioned previously, we aim to improve positioning accuracy of a flying robot through implementation of the first and second derivatives of acceleration in a navigation unit. Here an in-house-built quadrotor, equipped with an Ardupilot® and MPU6050 IMU sensors, is used for flight tests, and can be seen in Figure 6. To obtain the required data for computations, the quadrotor flew (in an assisted manner) for 90 seconds and measurements from the IMU were extracted. Mission Planner is the user interface software implemented in Ardupilot® with a variety of options. It can export flight data with extension “kmz”, which is utilised in Google Earth®. The reference path for flight tests is demonstrated in Figure 7(a), and the outputs produced by the kmz-file, during the test, are shown in Figure 7(b). This figure presents a whole view of the flight path in Google Earth®. Furthermore, accelerometer data and their corrections by complementary filter is available in Figure 8. Accelerometers are at risk to a number of problems such as being affected by noise, which in this case is rather common, in low-cost sensors. As discussed earlier, to avoid this problem, a low-pass filter is implemented. In the following, Figure 9 exhibits outputs of the gyros along three coordination axes. As stated, another type of exploited sensors are magnetometers, which measure the direction and intensity of Earth's magnetic field for attitude observations. By exploitation of the components of the gravity vector, it is possible to compute only roll and pitch angles, since, rotation about the z direction of the body frame is not observable from the components of the gravity acceleration. Figure 10 depicts magnetometer signals resolved in the body frame. The first process model and measurements/observations for the KF is for attitude estimation of the flying robot and Figure 11 illustrates them. As seen, KF estimates and corrects angles, desirably. Subsequently, Figures 12 and 13 give estimations of jerk and jounce components along x, y, and z directions in the body frame. Figure 14 illustrates the performance of the proposed algorithm on positioning accuracy in two dimensions, in the directions of east and north. Furthermore, Figure 15 illustrates the altitude time history, which exhibits the performance of the proposed algorithm on altitude estimations. Once one compares the outcomes with the reference values and barometer measurements, it is realised that the augmented KF improves the altitude results. Finally, Figures 16(a) to 16(c) present estimation errors in the directions of east, north, and altitude. Similarly, the outcomes evidently show that the augmented KF with the proposed method has a better performance in positioning, while the figures repeat this supremacy in correction of longitude, latitude, and altitude measurements.

Figure 6.

The exploited in-house-built quadcopter for flight tests.

Figure 7.

(a) 2D plot of reference path, (b) demonstration in Google Earth®.

Figure 8.

Accelerometer raw and filtered measurement data.

Figure 9.

Gyroscope measurement data.

Figure 10.

Magnetometer raw data.

Figure 11.

Estimation of Classical Euler angles (roll, pitch, and yaw).

Figure 12.

Estimation of jerk components along the Body frame's x, y, and z directions.

Figure 13.

Estimation of jounce components along the Body frame's x, y, and z directions.

Figure 14.

Estimated position components in the directions of East and North.

Figure 15.

Time history of altitude variations relative to the starting point.

Figure 16.

Position error in the directions of (a) East, (b) North, and (c) Up.