1. INTRODUCTION
The Strapdown Inertial Navigation System (SINS) estimates position, velocity, and attitude of a vessel using the signals measured by an Inertial Measurement Unit (IMU), which includes accelerometers and gyroscopes, based on the dead-reckoning principle (Titterton and Weston, Reference Titterton and Weston2004). Due to different imperfections in inertial sensors and consecutive integration of the measured signals, estimation error increases unboundedly with time (Farrell, Reference Farrell2008; Ali and Mirza, Reference Ali and Mirza2010). To restrict this estimation error, auxiliary sensors such as Global Navigation Satellite System (GNSS) receivers, Doppler Velocity Logs (DVL), inclinometers, Acoustic Positioning System (APS) and gyrocompasses could be used for marine navigation system applications (Vasilijevic et al., Reference Vasilijevic, Borovic and Vukic2012; Lee et al., Reference Lee, Jun, Kim, Lee, Aoki and Hyakudome2007; Miller et al., Reference Miller, Farrell, Zhao and Djapic2010).
The Kalman filter as a recursive algorithm that estimates the system states through noisy measurements has been widely implemented for integrating the SINS with auxiliary sensors. In the last decade, several methods based on this filter were proposed; for example, linearized Kalman filter, Unscented Kalman Filter (UKF), Particle Filter (PF), etc. The linearized Kalman filter has been used in two approaches: extended Kalman filter and Error State Kalman Filter (ESKF) (Lee et al., Reference Lee, Jun, Kim, Lee, Aoki and Hyakudome2007; Shabani et al., Reference Shabani, Gholami and Davari2015; Reference Shabani, Gholami, Davari and Emami2013). This filter applies the first order Taylor series for linearization of the non-linear dynamic system and observation models and assumes that noise follows a Gaussian distribution (Särkkä, Reference Särkkä2013). UKF as a free derivative method can be efficiently used in a problem with high nonlinearity and Gaussian noise. UKF estimates the nonlinear states by using a deterministic sampling approach and statistical linearization technique (Chang, Reference Chang2014; Li et al., Reference Li, Sun, Jia and Du2016). PF methods are recursive implementations of Monte-Carlo-based statistical signal processing. PF is not only used for nonlinear dynamic systems but is applicable to systems with non-Gaussian noise. In this method, the posterior probabilities are approximated by a set of weighted particles that are randomly selected (Wang and Ni, Reference Wang and Ni2016). The computational burden of UKF and PF, used in an integrated navigation system are higher than that for a linearized Kalman filter, because the sigma points must be propagated at a high frequency. This problem leads to restrictions on UKF and PF real-time applications. Using the PF method may cause other problems, such as degeneracy of the system performance and impoverishments of sample (Arulampalam et al., Reference Arulampalam, Maskell, Gordon and Clapp2002).
In this paper, among these methods, the ESKF method has been chosen due to lower computational load for real-time implementation. Moreover, for ESKF, initial tuning of parameters is not required, while initial setting of the parameters for PF, UKF, and hybrid filters needs trial and error or a high degree of experience. However, in all these methods, navigation system dynamics and statistical information of the process and measurement noise covariance matrix should be determined exactly to estimate the exact amount of states. Incorrect prior statistical knowledge may also lead to a divergence of the algorithm. Furthermore, the process and measurement noises must be white noise with a zero mean and their variances are required to be known for optimal estimation. In most applications, the exact values of variances of the process and measurement noises are unknown. On the other hand, optimum parameters of ESKF might vary from time to time. To overcome these issues, adaptive methods have been implemented to ESKF. The Adaptive Kalman Filter (AKF) is used to estimate process and measurement noises to adjust the parameters of the Kalman filter for optimal estimation according to the filter learning history (Mehra, Reference Mehra1970).
The procedures used to implement the AKF are classified into three main classes: Innovation-based Adaptive Estimator (IAE) (Magill, Reference Magill1965), Multiple Model Adaptive Estimation (MMAE) (Mehra, Reference Mehra1970) and Adaptive Fading Kalman Filter (AFKF) (Kim et al., Reference Kim, Jee, Park and Lee2009). In IAE, the adaptation uses new information from the filter innovation sequence to estimate the states and statistical information. This procedure is based on the covariance-matching technique amending the noise statistics by a consistent innovation covariance matrix with their theoretical values. MMAE is the combination of multiple elemental parallel Kalman filters, each of which has a distinct set of statistical information and it chooses the best state estimated by these simultaneously operating Kalman filters. Running the parallel filters simultaneously creates computational load. AFKF is based on the weighting of the error covariance equations with a scalar forgetting factor.
Some researchers have reported the application of one auxiliary sensor integrated with the SINS. AKFs have been applied mostly for SINS/GNSS integrated systems. Innovation-based AKF was used by (Mohamed and Schwarz, Reference Mohamed and Schwarz1999) for a SINS/GNSS integrated system. Li and Wang (Reference Li and Wang2013) designed an adaptive Kalman filter for Micro-Electromechanical System (MEMS)-IMU/magnetometers. Gao et al. (Reference Gao, Chen, Tao and Li2011) proposed an AKF for a SINS/DVL integrated system.
Some researchers have reported the implementation of multiple sensors with single rate; e.g. Lin and Hsueh (Reference Lin and Hsueh2013) applied a set of single rate multiple sensors for target tracking. Moreover, researchers have developed ESKF to implement it for a multirate multiple sensors integrated navigation system. For example, Lee et al. (Reference Lee, Jun, Kim, Lee, Aoki and Hyakudome2007) and Miller et al. (Reference Miller, Farrell, Zhao and Djapic2010) successfully implemented a multirate integrated navigation system using ESKF. The implementation details of ESKF for conventional integrated navigation system may be found in Farrell (Reference Farrell2008) and Lee et al. (Reference Lee, Jun, Kim, Lee, Aoki and Hyakudome2007).
Marine data integration is usually understood in two ways. Firstly, it means estimation of the ship state vector, by integrating IMU, GNSS, DVL and gyrocompass (Romanovas et al., Reference Romanovas, Ziebold and Lança2015), and secondly, it is used for target tracking in which usually radar and Automatic Identification System (AIS) are integrated (Kazimierski and Stateczny, Reference Kazimierski and Stateczny2013). Automatic tracking systems such as AIS and Automatic Radar Plotting Aids (ARPA) are used to detect and localise positions of ships and other vessels. These systems acquire information of position and direction of a vehicle using a GNSS or LORAN-C receiver and gyrocompass, respectively. Also, other sensors are required to obtain the roll and pitch angle values.
Although GNSS is available in marine navigation, it is not totally reliable in cases such as cloudy/stormy weather conditions or when subject to military applications such as spoofing or jamming (Grewal, Reference Grewal2011). Therefore, other auxiliary sensors like DVL, inclinometer and gyrocompass are implemented in integrated SINS. GNSS is used as a reference to calculate the estimation position error. The SINS as an independent system provides precision navigation over a short time without being influenced by the geographical conditions, traffic or environmental hazards. Since the update rates of the auxiliary sensors are different, it is not possible to use the conventional AKF for adaptively tuning the measurement covariance matrix. This paper reports, to the best of authors’ knowledge, the first development of a Multirate Adaptive Kalman Filter (MAKF) for integrated navigation systems. The procedure is based on innovation sequence and simultaneously estimates the statistical information of the measurement noise of asynchronous auxiliary sensors. Performance of MAKF is evaluated using real measurements and its results are compared with those of ESKF. In both methods, the integration structure is based on ESKF.
This paper is organised as follows. In Section 2, required equations for the navigation system are reviewed. Section 3 describes the procedures for the proposed multi-rate adaptive Kalman filter. In Section 4, performance of the proposed filter is evaluated using real data sets. Finally, conclusions are presented in Section 5.
2. EQUATIONS FOR NAVIGATION SYSTEM
In this section, firstly, the equations related to the position, velocity, orientation and bias in SINS are briefly reviewed. Afterwards, the system and measurement error state equations for implementing ESKF are described. The ESKF equations presented in this section (given by Lee et al., Reference Lee, Jun, Kim, Lee, Aoki and Hyakudome2007, Farrell (Reference Farrell2008), and Titterton and Weston (Reference Titterton and Weston2004)) have been changed so that they could be applied in the proposed MAKF for integrating an adaptive multi-rate multi-sensor navigation system.
2.1. The SINS Equations
In this paper, it is assumed that the output errors of an IMU are the sum of the bias and Gaussian random noise. The vehicle accelerations are obtained from the specific force measurements after compensating for both the Coriolis acceleration and the effect of gravity. The vehicle attitude is obtained from the angular rates that are measured by the gyroscope. The acceleration and angular rate vectors in the body frame (shown by
$\tilde{\bi f}^{b}$
and
$\tilde{\bf \omega}_{ib}^{b}$
, respectively), are modelled through Equations (1) and (2). In these equations,
${\bi f}^{b}$
and
${\bf \omega}_{ib}^{b}$
are the true acceleration and angular rate vectors and
b
and
w
are the bias and measurement noise, respectively. The measurement noises
w
a
and
w
g
are assumed to be Gaussian white noises with zero mean and variances
${\bf \sigma}_{a}^{2}$
and
${\bf \sigma}_{g}^{2}$
, respectively. The measured acceleration and angular rate vectors are corrected through Equations (3) and (4). In this paper, estimated quantities are denoted by a hat.
$$\tilde{\bi f}^{b} = {\bi f}^{b} + {\bi b}_{a} + {\bi w}_{a}$$
$$\tilde{\bf \omega}_{ib}^{b} = {\bf \omega}_{ib}^{b} + {\bi b}_{g} + {\bi w}_{g}$$
$$\hat{\bi f}^{b} = \tilde{\bi f}^{b} - \hat{\bi b}_{a}$$
$$\hat{\bf \omega}_{ib}^{b} = \tilde{\bf \omega}_{ib}^{b} - \hat{\bf b}_{g}$$
The accelerometer and gyroscope bias (
${\bi b}_{a}$
and
${\bi b}_{g}$
, respectively) are modelled as summations of random walks and random constants, as shown in Equations (5) and (6). In these equations,
${\bi b}_{\bi aC}$
and
${\bi b}_{\bi gC}$
are random constant vectors and
${\bi w}_{\bi ba}$
and
${\bi w}_{\bi bg}$
are driving noise vectors assumed to be Gaussian white noise with zero mean and known variances (
${\bf \sigma}_{ba}^{2}$
and
${\bf \sigma}_{bg}^{2}$
), respectively.
$${\bi b}_{\bi a} = {\bi b}_{\bi aC} + \int {\bi w}_{\bi ba}\,dt, \quad {\bi w}_{\bi ba} \sim N(0,{\bf \sigma}_{\bi ba}^{\bf 2})$$
$${\bi b}_{\bi g} = {\bi b}_{\bi gC} + \int {\bi w}_{\bi bg}\,dt, \quad {\bi w}_{\bi bg} \sim N(0, {\bf \sigma}_{\bi bg}^{\bf 2})$$
The state vector of the SINS given in Equation (7) in which
${\bi P}^{n} = [L, l]$
is the position vector of the vehicle in the navigation frame including latitude and longitude,
${\bi V}^{n} = [V_{N}, V_{E}, V_{D}]$
is the velocity vector resolved in the navigation frame in which is included north, east, and down velocities,
${\bf \varTheta} = [\phi, \theta, \varPsi]$
is the orientation vector composed of the roll, pitch and heading,
${\bi b}_{a} = [b_{ax}, b_{ay}, b_{az}]$
is the accelerometer's bias vector in the body frame and
${\bi b}_{g} = [b_{gx}, b_{gy}, b_{gz}]$
is the gyroscope drift vector in the body frame.
$${\bi x} ({\bi t}) = \lsqb {\bi P}^{n}, {\bi V}^{n}, {\bf \varTheta}, {\bi b}_{\bi a}, {\bi b}_{\bi g} \rsqb ^{T}$$
The navigation system state vector is estimated by numerical integration of the IMU signals and system dynamic equations. The equations related to the calculation of the position, velocity and orientation in a SINS can be expressed as Equations (8) to (10), in which, matrix
B
and a rotation matrix,
$\hat{\bf \varOmega}$
can be expressed in Equations (11) and (12). The change rates of the estimated bias of accelerometer and gyroscope can be expressed as Equations (13) and (14), where
$\hat{\bi R}_{b}^{n}$
is the transformation matrix from body frame to navigation frame.
$$\dot{\hat{\bi P}}^{n} = {\bi B} \hat{\bi V}^{\,n}$$
$$\dot{\hat{\bi V}}^{n} = \hat{\bi R}_{b}^{n} \hat{f}^{b} - \lpar 2 \hat{\bf \omega}_{ie}^{n} + \hat{\bf \omega}_{en}^{n} \rpar \times \hat{\bi V}^{\,n} + \hat{\bi g}^{n}$$
$$\dot{\hat{\bf \varTheta}} = \hat{\bf \varOmega}^{-1} (\hat{\bf \omega}_{ib}^{b} - \hat{\bi R}_{b}^{n} (\hat{\bf \omega}_{ie}^{n} + \hat{\bf \omega}_{en}^{n}))$$
$${\bi B} = \left[\matrix{1/(R_{N}) &0 &0 \cr 0 &Sec \, \hat{L}/(R_{E}) &0 \cr 0 &0 &1} \right]$$
$$\hat{\bf \varOmega} = \left[\matrix{1 &0 &-sin \hat{\theta} \cr 0 & cos \, \hat{\phi} & sin\, \hat{\phi}\, cos\, \hat{\theta} \cr 0 &-sin \,\hat{\phi} &cos\, \hat{\phi} \, cos\, \hat{\theta}} \right]$$
$$\dot{\hat{b}}_{\bi a} = 0$$
$$\dot{\hat{b}}_{\bi g} = 0$$
${\bf \omega}_{ie}^{n}$
,
${\bf \omega}_{en}^{n}$
and
${\bi g}^{n}$
are the angular rate of the Earth expressed in the navigation frame, the angular rate of the navigation frame with respect to the Earth-fixed frame, the gravity vector in the navigation frame, respectively as shown in Equations (15) to (17). E is the Earth rate
$(= 15\,{deg/h})$
and the variables R
N
and R
E
are the meridian radius and the transverse radius of the Earth's curvature.
$$\hat{\bf \omega}_{ie}^{n} = [E cos \hat{L} \,0\, - E\; sin\; \hat{L}]^{T}$$
$$\hat{\bf \omega}_{en}^{n} = \left[{\hat{V}_{E} \over R_{E}} - {\hat{V}_{N} \over R_{N}} - {\hat{V}_{E} \over R_{E}} tan \hat{L} \right]^{T}$$
$$\hat{\bi g}^{n} = [0\, 0\, g]^{T}$$
2.2. Error State Equations
The error state equation is represented as a time-varying linear system shown in Equation (18). In this equation
$\delta {\bi x} = {\bi x}_{true} - \hat{\bi x}$
represents the expected error between the true state,
${\bi x}_{\rm true}$
and its estimation,
$\hat{\bi x}$
. F is the dynamics matrix of the SINS which propagates the errors over time and
G
is the dynamic noise distribution matrix of the SINS. Moreover,
w
is process noise due to uncertainty in the control inputs modelled as a white noise.
$$\delta \dot{\bi x}(t) = {\bi F} (t) \delta {\bi x} (t) + {\bi G} (t) {\bi w}(t); {\bi w} (t) \sim N(0, {\bi Q}(t))$$
Equation (19) represents the matrix form of the error state equation. In the error state vector,
$\delta {\bi P}^{n} = [\delta L, \delta l]^{T}$
is the position error;
$\delta {\bi V}^{n} = [\delta V_{N}, \delta V_{E}, \delta V_{D}]^{T}$
is the velocity error;
${\bf \varPsi} = \lsqb \delta \alpha, \delta \beta, \delta \gamma \rsqb ^{T}$
is the orientation error; and
$\delta {\bi b}_{\bi a} = [\delta b_{ax}, \delta b_{ay}, \delta b_{az}]^{T}$
and
$\delta {\bi b}_{\bi g} = [\delta b_{gx}, \delta b_{gy}, \delta b_{gz}]^{T}$
are the accelerometers and gyroscope bias errors, respectively. In this equation
${\bi w}_{a}$
,
${\bi w}_{g}$
,
${\bi w}_{ba}$
and
${\bi w}_{bg}$
are noises due to uncertainty in the accelerometers, gyroscopes and their corresponding biases, respectively.
I
is a 3×3 identity matrix, and 0 is a 3×3 zero matrix.
$$\left[\matrix{\delta \dot{\bi P} \cr \delta \dot{\bi V} \cr \dot{\bf \varPsi} \cr \delta \dot{\bi b}_{a} \cr \delta \dot{\bi b}_{g}} \right] = \left[\matrix{{\bi F}_{11} &{\bi F}_{12} &{\bi F}_{13} &{\bf 0} &{\bf 0} \cr {\bi F}_{21} &{\bi F}_{22} &{\bi F}_{23} &R_{b}^{n} &{\bf 0} \cr {\bi F}_{31} &{\bi F}_{32} &{\bi F}_{33} &{\bf 0} &-R_{b}^{n} \cr {\bf 0} &{\bf 0} &{\bf 0} &{\bf 0} &{\bf 0} \cr {\bf 0} &{\bf 0} &{\bf 0} &{\bf 0} &{\bf 0}} \right] \left[\matrix{\delta {\bi P} \cr \delta {\bi V} \cr {\bf \varPsi} \cr \delta {\bi b}_{a} \cr \delta {\bi b}_{g}} \right] + \left[\matrix{{\bf 0} & {\bf 0} & {\bf 0} & {\bf 0} \cr {\bi R}_{n}^{b} &{\bf 0} &{\bf 0} &{\bf 0} \cr {\bf 0} &-{\bi R}_{n}^{b} &{\bf 0} &{\bf 0} \cr {\bf 0} &{\bf 0} &-{\bi I} &{\bf 0} \cr {\bf 0} &{\bf 0} &{\bf 0} &-{\bi I}} \right] \left[\matrix{{\bi w}_{a} \cr {\bi w}_{g} \cr {\bi w}_{ba} \cr {\bi w}_{bg}} \right]$$
In Equation (19), the components of the dynamics matrix,
${\bi F}_{ij}$
for the SINS (given in the appendix) are derived from Equations (20)–(24). These equations are the linearized equations related to the errors of the position, velocity, orientation and bias, respectively. In these equations,
$\delta {\bi f}^{b}$
and
$\delta {\bf \omega}_{ib}^{b}$
indicate the errors in the signals measured by accelerometers and gyroscopes. The angular rate error of the Earth rotation, expressed in the navigation frame and the angular rate error of the navigation frame rotation with respect to the Earth-fixed frame, are denoted by
$\delta {\bf \omega}_{ie}^{n}$
and
$\delta {\bf \omega}_{en}^{n}$
, respectively. Furthermore,
$\delta {\bi g}^{n}$
is the gravity vector error in the navigation frame,
${\bf \varPsi}$
is orientation error vector (on condition that small angle approximations are valid) assuming to be equal to the error of the Euler angles (i.e. the roll, pitch, and yaw errors). Error in the angular rate of the navigation frame with respect to the inertial frame is denoted by
$\delta {\bf \omega}_{in}^{n}$
.
$$\delta \dot{\bi P}^{n} = \delta {\bi V}^{n}$$
$$\delta \dot{\bi V}^{n} = \lsqb {\bi f}^{n} \times \rsqb {\bf \varPsi} + {\bi R}_{b}^{n} \delta {\bi f}^{b} - \lpar 2{\bf \omega}_{ie}^{n} + {\bf \omega}_{en}^{n}\rpar \times \delta {\bi V}^{n} - \lpar 2 \delta {\bf \omega}_{ie}^{n} + \delta {\bf \omega}_{en}^{n} \rpar \times {\bi V}^{n} - \delta {\bi g}^{n}$$
$$\dot{\bf \varPsi} = -{\bf \omega}_{in}^{n} \times {\bf \varPsi} + {\bf \delta} {\bf \omega}_{in}^{n} - {\bi R}_{b}^{n} \delta {\bf \omega}_{ib}^{b}$$
$$\delta \dot{\bi b}_{\bi a} = -{\bi w}_{a}$$
$$\delta \dot{\bi b}_{\bi g} = -{\bi w}_{g}$$
2.3. The Measurement Equations
The measurement error equation of ESKF is expressed in Equation (25) in which,
$\delta {\bi y}_{k}$
is a ς-dimensional measurement error vector. In integrated navigation systems, the measurement error vector is the difference between the navigation solution from the SINS and the measurement of auxiliary sensors. These measurements are the linear combination of states perturbed by measurement noise.
${\bi H}_{k}$
and
${\bi M}_{k}$
are measurement output and noise distribution of the measurement matrices, respectively and
${\bi r}_{k}$
represents the measurement noise assumed to be a zero mean Gaussian white noise with time-varying covariance matrix,
${\bi R}_{k}$
.
$$\delta {\bi y}_{k} = {\bi H}_{k} \delta {\bi x}_{k} + {\bi M}_{k}{\bi r}_{k}, \quad {\bi r}_{k}\sim N(0, {\bi R}_{k})$$
In the best conditions, if all possible measurement sensors being used in navigation systems, including GNSS for horizontal position, DVL for the velocity, inclinometer for the attitude, and gyrocompass for heading are available, vector
${\delta {\bi y}}_{k}$
and matrices
${\bi H}_{k}$
,
${\bi M}_{k}$
and
${\bi R}_{k}$
can be stated as Equations (26) to (29), in which ς is four.
$${\delta {\bi y}}_{k} = \lsqb \delta {\bi y}_{k,1}^{T}, \delta {\bi y}_{k,2}^{T},\ldots, \delta {\bi y}_{k,\varsigma}^{T} \rsqb ^{T}$$
$${\bf H}_{k} = \lsqb {\bf H}_{k,1}^{T}, {\bf H}_{k,2}^{T}, \ldots, {\bf H}_{k,\varsigma}^{T} \rsqb ^{T}$$
$${\bf M}_{k} = \lsqb {\bf M}_{k,1}^{T}, {\bf M}_{k,2}^{T}, \ldots, {\bf M}_{k,\varsigma}^{T}\rsqb ^{T}$$
$${\bf R}_{k} = diag \lpar {\bi R}_{k,1}, {\bi R}_{k,2}, \ldots, {\bi R}_{k,\varsigma} \rpar $$
At the k th time step, if the first sensor (GNSS) data is available, the measurement error of the horizontal position can be expressed by using Equations (30) and (31). In these equations, L m and l m are the latitude and longitude measured by the GNSS and r L and r l are the measurement noise, with variances σ L 2 and σ l 2. Measurement error equation in this case may be given as Equation (32). It should be noted that in this paper, GNSS is used only for alignment and as a reference and is not used in the navigation phase.
$$\delta y_{L} = \hat{L} - L_{m} = \lpar L_{true} + \delta L\rpar - \lpar L_{true} + r_{L}\rpar = \delta L - r_{L}$$
$$\delta y_{l} = \hat{l} - l_{m} = \lpar l_{true} + \delta l\rpar - \lpar l_{true} + r_{l}\rpar = \delta l - r_{l}$$
$$\left[\matrix{\delta y_{L} \cr \delta y_{l}} \right] = \left[\matrix{1 &0 &{\bf 0}_{1\times 12} \cr 0 &1 &{\bf 0}_{1\times 12}} \right] \delta {\bi x}_{k} + \left[\matrix{1 &0 &{\bf 0}_{1\times 6} \cr 0 &1 &{\bf 0}_{1\times 6}} \right] \left[\matrix{r_{L} \cr r_{l}} \right]$$
When the second sensor (DVL) data is received, the relationship between the measurement error and the error state of the velocity are expressed as Equations (33) and (34), where
${\bi r}_{{\bi V}^{\bi b}}$
is the measurement noise of the DVL and
$\sigma_{vx}^{2}$
,
$\sigma_{vy}^{2}$
and
$\sigma_{vz}^{2}$
are their variances. The auxiliary signals of the velocity measured by the DVL are denoted as
$(V_{NM}, V_{EM}, V_{DM})$
, determined in the navigation frame.
$$\delta {\bi y}_{k,2} = \hat{\bi V}^{\,n} - {\bi V}_{m}^{n} = \lpar {\bi V}_{true}^{n} + \delta {\bi V}^{n} \rpar - \hat{R}_{b}^{n}\lpar {\bi V}_{true}^{b} + {\bi r}_{\bi V}^{\bi b} \rpar = \delta {\bi V}^{n} - \hat{V}^{n} \times {\bf \varPsi} - \hat{\bi R}_{b}^{n} {\bi r}_{{\bi V}^{\bi b}}$$
$$\eqalign{\delta {\bi y}_{k,2} = \left[\matrix{\hat{\bi V}_{N} - {\bi V}_{Nm} \cr \hat{\bi V}_{E} - {\bi V}_{Em} \cr \hat{\bi V}_{D} - {\bi V}_{Dm}} \right] = \left[\matrix{{\bf 0}_{1\times 2} &1 &0 &0 &0 &-\hat{V}_{D} &\hat{V}_{E} &{\bf 0}_{1\times 6} \cr {\bf 0}_{1\times 2} &0 &1 &0 &\hat{V}_{D} &0 &-\hat{V}_{N} &{\bf 0}_{1\times 6} \cr {\bf 0}_{1\times 2} &0 &0 & 1 &-\hat{V}_{E} &\hat{V}_{N} &0 &{\bf 0}_{1 \times 6}} \right] \delta {\bi x}_{k} \cr &\quad + \left[\matrix{{\bf 0}_{3\times 2} &\hat{{\bi R}}_{b}^{n} &{\bf 0}_{3\times 3}} \right]{\bi r}_{{\bi V}^{\bi b}}}$$
The inclinometer measurements are computed by the accelerometer signals. Although, from a theoretical point of view, the processing of the same measurements in both system equations and measurement equations is not completely correct, this technique works properly in practice (Georgy et al., Reference Georgy, Karamat, Iqbal and Noureldin2011). When the vessel's velocity is approximately constant, the only acceleration acting on the vessel is the acceleration due to gravity. Thus, the roll and pitch signals may be computed by using Equations (35) and (36) (Vaganay and Aldon, Reference Vaganay and Aldon1994), where
${\bf \phi}_{\rm m}$
and θ
m
are the auxiliary signals of orientation measured by the inclinometer and f
x
and f
y
are the specific force along the forward and starboard sides measured by the accelerometers.
$$\theta_{m} = {sin^{-1}(f_{x}) \over g}$$
$$\phi_{m} = -{sin^{-1}\lpar f_{y}\rpar \over g\ cos\ \theta_{m}}$$
The relationship between the measurement error and error states of the attitude is presented in Equations (37) to (39); and the difference between the measurement and the states of attitude may be given by Equation (40). In these equations,
${\bf \varPhi} = [\phi, \theta]^{T}$
is the attitude (auxiliary signals of roll and pitch) and
${\bi r}_{\bf \phi}$
is the measurement noise of the inclinometer while
$\sigma_{\phi}^{2}$
and
$\sigma_{\theta}^{2}$
are their variances.
$$\delta {\bi y}_{k,3} = \hat{\bf \varPhi} - {\bf \varPhi}_{\bi m} = \hat{\Phi} - \lpar {\bf \varPhi}_{\bi true} + {\bi r}_{\bf \varPhi} \rpar = \lpar \hat{\Phi} - {\bf \varPhi}_{\bi true} \rpar - {\bi r}_{\bf \varPhi}$$
$$\hat{\bf \varPhi} - {\bf \varPhi}_{true} = -\left[\matrix{1 &0 &0 \cr 0 &1 &0} \right]{\bf \varGamma}^{-1} {\bf \varPsi}$$
$${\bf \varGamma} = \left[\matrix{cos\ \psi\ cos\ \theta &-sin \psi &0\cr sin \psi cos \theta & cos \psi &0 \cr -sin \theta &0 &1} \right]$$
$$\eqalign{\left[\matrix{\hat{\phi} - \phi_{m} \cr \hat{\theta} - \theta_{m}} \right] &= \left[\matrix{{\bf 0}_{1\times 5} - {\cos \phi \over \cos \, \theta} & \sin \psi \tan \theta \cos \psi &{\bf 0}_{1 \times 6} \cr {\bf 0}_{1\times 5} - {\sin \psi \over \cos\, \theta} &-\cos \psi\,{-}\tan \theta \sin \psi &{\bf 0}_{1 \times 6}} \right] \delta {\bi x}_{k} \cr &\quad + \left[\matrix{{\bf 0}_{1 \times 5} &-1 &0 &0 \cr {\bf 0}_{1 \times 5} &0 &-1 &0 } \right] {\bi r}_{{\bf \phi}}}$$
When the fourth sensor (gyrocompass) data is available, the relationship between measurement error and error states of the heading can be expressed as Equations (41) and (42), in which r
ψ is measurement noise of the gyrocompass and
$\sigma_{\psi}^{2}$
is its variance. In these equations, ψ
m
is the auxiliary signal of heading measured by the gyrocompass.
$$\delta y_{k,4} = \hat{\psi} - \psi_{m} = \hat{\psi} - \lpar \psi_{true} + r_{\psi}\rpar = -\left[\matrix{0 &0 &1} \right]{\bf \varGamma }^{-1}\psi - r_{\psi}$$
$$\hat{\psi} - \psi_{m} = \left[\matrix{{\bf 0}_{1\times 5} &-1 &{\bf 0}_{1\times 6}} \right] \delta {\bi x}_{k} + \left[\matrix{{\bf 0}_{1\times 7} &-1} \right] r_{\psi}$$
When data for all four auxiliary sensors are available, the measurement covariance matrix can be expressed as Equation (43).
$${\bf R}_{k} = diag (\sigma_{L}^{2}, \sigma_{l}^{2}, \sigma_{v_{x}}^{2}, \sigma_{v_{y}}^{2}, \sigma_{v_{z}}^{2}, \sigma_{\phi}^{2}, \sigma_{\theta}^{2}, \sigma_{\psi}^{2})$$
3. MULTIRATE ADAPTIVE KALMAN FILTER
Real application of ESKF in navigation systems is limited by the availability of a priori statistics of the process and measurement noises. Moreover, it should be assumed that these statistics are constant, while the noise statistics may change from time to time due to environment and system dynamics. Insufficient knowledge of a priori filter statistics leads to a reduction of the filter accuracy or a loss of its convergence (Saha et al., Reference Saha, Ghosh and Goswami2014). AKF is a solution for these problems. It estimates measurement and process covariance matrices adaptively through the filter learning process (Maybeck, Reference Maybeck1982).
Since in most applications of navigation systems a set of auxiliary sensors with different sampling rates are applied, the conventional AKF cannot be executed. In this paper, MAKF is proposed to estimate the statistical information of the process and measurement noise using asynchronous auxiliary sensors. In order to formulate the MAKF, Equations (18) and (25) are required. For applying MAKF, one needs to discretize error state Equation (18), by converting it to Equation (44). Because the time interval between two successive IMU samplings
$(\varDelta t)$
is too short (in this paper 0·01 s), it can be assumed that
${\bi F}(t)$
in the time interval between k
th and (k+1)th time steps is constant and equal to
${\bi F}(t)$
in the k
th time step. Therefore the state transition matrix
${\bi A}_{k}$
can be calculated by using Equation (45). Moreover, because
$\varDelta t$
is much smaller than one second, Equation (45) can be approximated by using the second order term of the Taylor series, presented in Equation (46) (Farrell, Reference Farrell2008). In this equation,
${\bi L}_{k}$
is the dynamic noise distribution matrix,
${\bi w}_{k}$
is input white noise with a zero-mean Gaussian distribution, and
${\bi Q}_{k}$
is its covariance matrix.
$$\delta {\bi x}_{k} = {\bi A}_{k - 1}\delta {\bi x}_{k - 1} + {\bi L}_{k - 1}{\bi w}_{k - 1}, \quad {\bi w}_{k} \sim N(0,{\bi Q}_{k})$$
$${\bi A}_{k} = {\bi A}\lpar t = k: t = k + 1 \rpar = \exp ({\bi F} \lpar t = k \rpar \Delta t)$$
$${\bi A}_{k} = {\bi I} + {\bi F} \lpar t = k \rpar \Delta t + 0.5{\bi F}^{2} \lpar t = k \rpar \Delta t^{2}$$
The MAKF (presented in Figure 1) consists of two main stages including prediction and update procedures. In the prediction procedure, the system state and its covariance are predicted receiving measured inertial data as shown in Equations (47) and (48). To estimate the state noise statistics,
${\bi Q}_{\rm k}$
, adaptively, the state noise sample at the k
th
time step is defined as Equation (49) and the covariance of state noise sample is given as Equation (50) (Myers and Tapley, Reference Myers and Tapley1976), where
$\hat{\bi q}$
is the mean value of the state noise sample, and N is the size of the moving estimation window. Unbiased estimation of the process noise covariance matrix can be expressed by Equation (51) (Myers and Tapley, Reference Myers and Tapley1976).
The flowchart of the MAKF.
$$\delta \hat{\bi x}_{k}^{-} = {\bi A}_{k - 1} \delta \hat{\bi x}_{k}^{+}$$
$${\bi P}_{k}^{-} = {\bi A}_{k-1} {\bi P}_{k-1}^{+} {\bi A}_{k-1}^{T} + \hat{\bi Q}_{k-1}$$
$${\bi q}_{k} = \hat{\bi x}_{k}^{+} - {\bi A}_{k-1} \hat{\bi x}_{k-1}^{+}$$
$$\hat{\bi C}_{{\bi q}_{k}} = {1 \over N - 1} \sum_{j = k - N + 1}^k \lpar {\bi q}_{j} - \hat{\bi q} \rpar \lpar {\bi q}_{j} - \hat{\bi q} \rpar ^{T}$$
$$\hat{\bi Q}_{k} = \hat{\bi C}_{{\bi q}_{k}} - {1 \over N} \sum_{j = k - N + 1}^k \lpar {\bi A}_{j} {\bi P}_{j-1}^{+} {\bi A}_{j}^{T} - {\bi P}_{j}^{+} \rpar \approx \hat{\bi C}_{{\bi q}_{k}}$$
The update procedure of the MAKF is executed when one or more of the auxiliary sensors’ data are received at the k
th
time step. When no auxiliary sensors’ data is received, only the prediction procedure is performed. Receiving a new auxiliary measurement, y
k
at the k
th
time step, the navigation error state,
$\hat{\bi x}_{k}^{+}$
and its associated covariance matrix,
${\bi P}_{k}^{+}$
are corrected.
In this paper, the update procedure of the MAKF is based on an innovation sequence. The statistical algorithm of the adaptive Kalman filter based on innovation sequence is gained using the covariance matching approach. The innovation sequence is simply the difference between the actual measurements and the predicted measurements based on the predicted state vector. The innovation sequence at the k th time step is defined by Equation (52). Assuming an ergodic and stationary innovation sequence, the autocorrelation of innovation sequence is computed by averaging inside a moving window of size N, as shown in Equation (53). The measurement noise covariance matrix is estimated based on innovation sequence by using the rule of covariance matching as shown in Equation (54).
$${\bi v}_{k} = {\bi y}_{k} - {\bi H}_{k} \hat{\bi x}_{\bi k}^{-}$$
$$\hat{\bi C}_{v_k} = {1 \over N} \sum_{j = 1}^N {\bi v}_{k-j} {\bi v}_{k-j}^{T}$$
$$\hat{\bi R}_{k} = \hat{\bi C}_{v_k} + {\bi H}_{k} {\bi P}_{k}^{-} {\bi H}_{k}^{T}$$
Due to different sampling rates of the auxiliary sensors, the length of the vector v changes within the moving window, thus, C vk in Equation (53) could not be calculated. In order to fix this problem, the dimension of the innovation sequence is considered constant and equal to the number of states. When data from all the auxiliary measurements are received, the innovation sequence is expressed as Equation (55). The innovation sequence is considered based on the received auxiliary measurements, and for the sensors without received data at the k th time step, their corresponding innovation values in the vector are replaced by the previous value.
$${\bi v}_{k} = \left[\matrix{{\bi y}_{k,1} - {\bi H}_{k,1} \hat{\bi x}_{k,1}^{-} \cr {\bi y}_{k,2} - {\bi H}_{k,2} \hat{\bi x}_{k,2}^{-} \cr \vdots \cr {\bi y}_{k,\varsigma} - {\bi H}_{k,\varsigma} \hat{\bi x}_{k,\varsigma}^{-}} \right]$$
As a necessary and sufficient condition for optimal performance of innovation-based MAKF, the evaluated innovation sequences should be white. If noises of the measurement and the system are white, the innovation sequences will be white (Mehra, Reference Mehra1970). In a non-white sequence, the correlation coefficients in non-zero lags exist and a whitening process is required (Mehra, Reference Mehra1970). A statistical test developed by Magill (Reference Magill1965), to check whether innovation sequence is white or not, is used in the proposed MAKF.
To improve the MAKF stability, the estimated autocorrelation of innovation sequence should be positive definite. To guarantee the positive definiteness of
$\hat{\bi C}_{vk}$
, the Singular Value Decomposition (SVD) method is used (Grewal, Reference Grewal2011). If
$\hat{\bi C}_{vk}$
is a symmetric matrix, its SVD is calculated by an orthonormal matrix, U a diagonal matrix, S and conjugate transpose of the orthonormal matrix V, as shown in Equation (56).
$$\hat{\bi C}_{vk} = {\bi U}\ {\bi S}\ {\bi V}^T$$
In Equation (53), matrix dimension of
$({\bi H}_{k}\ {\bi P}_{k}^{-}\ {\bi H}_{k}^{T})$
is equal to the number of available measurements different from the dimension of
$\hat{\bi C}_{vk}$
which is equal to the number of states. Thus,
${\bi R}_{k}$
could not be calculated. To resolve this problem,
$\hat{\bi C}_{vk}$
is modified using
$\lsqb {\bi H}_{k}\; \hat{\bi C}_{vk}\; {\bi H}_{k}^{T} \rsqb $
. The modified measurement noise covariance matrix is given by Equation (57). The update procedure of the MAKF is given by Equations (58) to (60). In these equations,
${\bi K}_{k}$
is the Kalman gain which updates the weight between auxiliary measurements and the predicted states.
$$\hat{\bi R}_{k} = {\bi H}_{k}\ U\ \sqrt{S}\ U^{T} {\bi H}_{k}^{T} + {\bi H}_{k}\ {\bi P}_{k}^{-}\ {\bi H}_{k}^{T}$$
$${\bi K}_{k} = {\bi P}_{k}^{-}\ {\bi H}_{k}^{T} \lpar {\bi H}_{k}\ {\bi P}_{k}^{-}\ {\bi H}_{k}^{T} + {\bi M}_{k}\ \hat{\bi R}_{k}\ {\bi M}_{k}^{T} \rpar ^{-1}$$
$$\delta \hat{\bi x}_{k}^{+} = \delta \hat{\bi x}_{k}^{-} + {\bi K}_{k}\ {\bi v}_{k}$$
$${\bi P}_{k}^{+} = {\bi P}_{k}^{-} - {\bi K}_{k}\lpar {\bi H}_{k}\ {\bi P}_{k}^{-}\ {\bi H}_{k}^{T} + {\bi M}_{k}\ \hat{\bi R}_{k}\ {\bi M}_{k}^{T} \rpar {\bi K}_{k}^{T}$$
4. EXPERIMENTAL RESULTS
In this paper, to assess performance of the proposed MAKF, a lake test was performed on a sunny and wind-free day using the instrumented vessel shown in Figure 2. The measurement instruments used in the test shown in Figure 3 include a tactical grade IMU, a DVL, a GNSS receiver, a gyrocompass with an accuracy of a tenth of a degree to get the initial value of heading and a triple combination of three orthogonal accelerometers as inclinometer sensor to measure attitude. The tactical grade IMU has three MEMS accelerometers and three fibre optic gyroscopes. The GNSS measurements are used only to obtain the initial values of the position and as the reference trajectory (i.e. GNSS is not used as an auxilliary sensor). Since the GNSS is a line of sight sensor and can be affected by interference, jamming and stormy weather conditions, reliability and continuity of signal cannot always be guaranteed. Since GNSS measurements are of high accuracy, if its data was integrated in our navigation system, the performance of our algorithm could not be evaluated. Therefore we used the GNSS data only as a reference to evaluate our algorithm.
Instrumented vessel for experiments.
Instruments mounted on the vessel.
A dynamic test is conducted to better evaluate performance of the proposed MAKF and compare it with the ESKF method. In this test, the inertial sensor signals are integrated with measurements of the DVL and inclinometer. The technical specifications of the instruments are shown in Table 1. In order to log the signals measured by the instruments, an industrial computer was used that is carried by the vessel. A stand was used to hold the DVL under the water, which can be seen in Figure 3.
The technical specification of the instruments.
The accuracy of the estimated attitude is determined through the accelerometers’ biases so that each milli-g of accelerometer bias leads to about 1 mili-rad of error to the attitude estimate (Farrell, Reference Farrell2008). In order to determine the initial values of the state variables, in stationary condition coarse alignment is performed in the initial phase. Then the fine alignment phase is carried out to find deterministic inertial sensors errors and attitude errors using auxiliary measurements in the stationary condition (Ali and Ushaq, Reference Ali and Ushaq2009). At the end of the fine alignment, the navigation phase is started where the proposed MAKF estimates optimally the system's state using auxiliary measurements provided by the DVL and inclinometer. To validate the performance of the MAKF, the conventional error state Kalman filter is also implemented and its results are compared with those of the MAKF.
In this test, the vessel travelled approximately 4,430 m for nearly 50 min. It had been in a stationary condition for 10 minutes at the beginning of the trajectory. Figure 4 shows the trajectory estimated in the horizontal (i.e. north and east) directions by the ESKF and the proposed MAKF. The reference trajectory is the smoothed data of the GNSS output in the north and east directions.
Estimated trajectory by MAKF and ESKF.
Figures 5 and 6 compare error of the estimated trajectory by the MAKF and ESKF in latitude and longitude directions respectively. The error is defined as the difference between the reference and estimated latitude (or longitude), at each time step. The Root Mean Square Error (RMSE) values of the estimated position in the directions of the north and east are 8·8 m and 4·3 m for the proposed MAKF and 13·9 m and 19·5 m for the ESKF.
Error of the estimated latitude for the MAKF and ESKF.
Error of the estimated longitude for the MAKF and ESKF.
To compare the performance of MAKF and ESKF quantitatively, RMSE and relative RMSE are presented in Equations (61) and (62), where (x ref , y ref , z ref ) and (x est , y est , z est ) are the reference and estimated position in x, y and z directions. As a result, the RMSEs of the position estimated by the MAKF and ESKF are 9·8 and 24 m and relative RMSE are 0·22% and 0·56% of the travelled distance, respectively. According to these results, the MAKF improves the accuracy of the estimated position by approximately 60% compared to the ESKF.
$$RMSE = \sqrt{{1 \over n} \sum_{i = 1}^n \left[ \lpar x_{ref}^{i} - x_{est}^{i}\rpar^{2} + \lpar y_{ref}^{i} - y_{est}^{i}\rpar^{2} + \lpar z_{ref}^{i} - z_{est}^{i}\rpar^{2}\right]}$$
$$Relative RMSE = {RMSE \over Travelled Distance} \times 100$$
In Figure 7, the absolute errors of the estimated position are shown. At any moment, the absolute error is defined as the Euclidean distance between the reference and the estimated positions. This figure clearly shows that the positioning errors with the MAKF are significantly smaller and smoother than the ESKF. Refering to Figure 7, maximum absolute error for the ESKF is more than 80 m, while it is as low as 22 m for the MAKF. This means that under identical conditions, the performance of the MAKF remains more stable.
Absolute error of the estimated position.
Figure 8(a) shows the velocity of the vessel measured by the DVL, and Figure 8(b) shows the measurement noise variance of the DVL estimated by the MAKF. Figure 9 shows the noise variance of the inclinometer estimated by the MAKF. According to these figures, estimation of measurement noise variance is consistent with real variations of the measurement noise and also changes along with time. Moreover, the values of the measurement noise variance in the stationary condition are small and they increase when the vessel starts to move. Their fluctuations in the moving condition are also larger than those in the stationary condition.
(a) Velocity measured by DVL. (b) Estimated measurement noise variance of the DVL.
Estimated measurement noise variance of the inclinometer.
Figure 8(a) and Figure 7 indicates that the ESKF is sensitive to external disturbance. For example, the estimation stability of the ESKF degrades obviously, in cases where the DVL signal has a sudden and dramatic change due to environmental factors. Outliers in DVL output occuring between 2000 to 3000 seconds shown in Figure 8(a) are responsible for the error increase during the same time interval of ESKF estimation shown in Figure 7. At the same time, the MAKF errors shown in Figure 7 are lower and smoother compared to the ESKF and it is obvious that the MAKF is robust in dealing with sudden and large changes of measurements of DVL.
Figure 10 compares the estimated heading by the MAKF and the ESKF and Figure 11 compares the errors of the estimated heading by MAKF and ESKF. It is clear that the MAKF estimates the heading more accurately and with higher stability compared to the ESKF. While the estimation error of the MAKF is more consistant, the estimation fluctuations of the ESKF is obvious in Figure 7. The RMSEs of the estimated heading by the MAKF and ESKF are
$0{\cdot}31^{\circ}$
and
$1{\cdot}42^{\circ}$
, respectively. The heading reference is measured by a gyrocompass which is used only as a reference. The gyrocompass is used only in the coarse alignment phase.
Estimated Heading by MAKF and ESKF.
Error of estimated Heading by MAKF and ESKF.
5. CONCLUSION
This paper has presented a multirate adaptive Kalman filter based on an innovation sequence to enhance the accuracy of the ESKF for marine integrated navigation systems with time-varying unknown noise statistics. In the MAKF, the measurement noise covariance matrix is tuned adaptively for marine multirate integrated navigation systems using an innovation-based adaptive estimation. For this purpose, the autocorrelation function of the innovation sequence is calculated by replacing data with the previous value of the elements corresponding to the non-received auxiliary measurements in the innovation sequence. Afterwards, the equation related to the matrix R is modified. The results of the lake test show that the RMSE of the position estimated by the MAKF is reduced by about 60% compared to the ESKF. Results also show that the proposed MAKF can effectively suppress the impact of unstable DVL measurement noise and is robust against interference and outliers compared to the ESKF. Therefore the MAKF estimates the system states of navigation with high accuracy by estimating the appropriate value for the noise covariance of the auxiliary sensors.
APPENDIX
The dynamics matrices F ij of the SINS Equation (19) are given by Equations (A1) to (A9).
$${\bi F}_{11} = \left[\matrix{0 &0 &-{V_{N}\over R_{N}^{2}} \cr {V_{E}\tan L \over R_{E}\cos L} &0 &{V_{E} \over R_{E}^{2}\cos L} \cr 0 &0 &0} \right]$$
$${\bi F}_{12} = \left[\matrix{{1 \over R_N} &0 &0 \cr 0 &{1 \over R_{E}\cos L} &0 \cr 0 &0 &1} \right]$$
$${\bi F}_{13} = \left[\matrix{0 &0 &0 \cr 0 &0 &0 \cr 0 &0 &0} \right]$$
$${\bi F}_{21} = \left[\matrix{-V_{E}\left(2\varOmega \sin L + {V_{E} \over R_{E} \cos L^{2}}\right) &0 &{1 \over R_{E}^{2}} \lpar V_{E}^{2}\tan L-V_{N}V_{D}\rpar \cr 2\varOmega \lpar V_{N}\cos L - V_{D}\sin L \rpar + {V_{N}V_{E} \over R_{E}\cos L^{2}} &0 &{V_{E} \over R_{E}^{2}} \lpar V_{N} \tan L + V_{D} \rpar \cr 2\varOmega V_{E} \sin L &0 &{1\over R_{E}^{2}} \lpar V_{N}^{2} + V_{E}^{2}\rpar } \right]$$
$${\bi F}_{22} = \left[\matrix{{V_{D} \over R_{N}} &-2\left(\varOmega \sin L + {V_{E} \over R_{E}} \tan L \right) &{V_{N} \over R_{N}} \cr (2\varOmega \sin L + {V_{E} \over R_{E}} \tan L) &{1 \over R_{N}}\lpar V_{N}\tan L + V_{D}\rpar &\left(2\varOmega \cos L + {V_{E} \over R_{E}} \right) \cr -{2V_{N} \over R_{N}} &-2\left( \varOmega \cos L + {V_{E} \over R_{E}} \right) &0} \right]$$
$${\bi F}_{23} = \left[\matrix{0 &-f_{D} & f_{E} \cr f_{D} &0 &-f_{N}\cr -f_{E} & f_{N} &0} \right]$$
$${\bi F}_{31} = \left[\matrix{-\varOmega \sin L &0 &{-V_{E} \over R_{E}^{2}} \cr 0 &0 &{V_{N} \over R_{N}^{2}} \cr -\varOmega \cos L - {V_{E} \over R_{E}} \cos L^{2} &0 &{V_{E} \over R_{E}^{2}} \tan L} \right]$$
$${\bi F}_{32} = \left[\matrix{0 &{1 \over R_{E}} &0 \cr {-1 \over R_N} &0 &0 \cr 0 &{-\tan L \over R_E} &0} \right]$$
$${\bi F}_{33} = \left[\matrix{0 &-\left(\varOmega \sin L + {V_{E} \over R_E} \tan L \right) &{V_N \over R_N} \cr \left(\varOmega \sin L + {V_{E} \over R_{E}} \tan L\right) &0 &\left(\varOmega \cos L + {V_{E} \over R_{E}}\right) \cr -{V_{N} \over R_{N}} &-\varOmega \cos L - {V_{E} \over R_{E}} &0} \right]$$
