1. INTRODUCTION
The integration of systems includes estimation of internal variables in the systems not only by the mixing of homogeneous data, but also by the integration of heterogeneous data produced at each system. A filtering problem that is approached using dynamic and measurement models in the system for estimation has been studied by multi-faceted methods for linear and non-linear systems which have various probabilistic and statistical properties. Although a conceptually optimal filter for non-linear systems is a Bayesian filter, it has an intractable difficulty in the calculation of variables in a probability density function for multivariate systems. Thus, a suboptimal solution of the Bayesian filter is regarded as a satisfactory result. The suboptimal solution can be divided into two: a method that assumed a posterior density via Gaussian, and a method that does not assume a posterior density. The former method includes Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), and the latter method includes Point-mass filter, Gaussian Mixture filter, Particle filter etc. (Gelb 1974, Maybeck 1979, Julier et al. 2000, Ristic et al. 2004). Although the latter method is basically better in estimation performance than the former method, it requires a large computation amount and thus has a limitation in being implemented in a real-time operation. Accordingly, EKF has been utilized as a general filter that is applicable to nonlinear systems, and UKF proposed by Julier et al. (2000) has been known to have better performance than the EKF when the initial estimation error is large, which has been studied much until recently.
The Cubature Kalman Filter (CKF) proposed by Arasaratnum & Haykin in 2009 is a filter induced from multivariable integrals revealed during time propagation of the Bayesian filter and measurement update process using the characteristic of efficient numerical integration method known as the cubature rule (Arasaratnam & Haykin 2009). The previous studies explained that the proposed three-degree CKF has a similar characteristic with that of the UKF, but it resolves a problem that does not always ensure positive definite in the co-variance matrix due to the use of negative weight and round-off error, which can be generated in the UKF (Arasaratnam & Haykin 2009, Jia et al. 2013). Thus, the CKF was used as a basic filter in this paper.
Generally, a data output rate between systems is different when the CKF is used in system integration. If an output rate in the main system is high and that of auxiliary system is low, time propagation of the filter is done fast while the measurement update is done slowly. Here, time propagation of cubature point (CP) of the CKF of which number is twice of the system degree is conducted in accordance with an output rate of the main system, in which much computation load occurs. In particular, when non-linear functions are complex in high-degree systems, it is difficult to achieve a real-time implementation. In this paper, a simplified CKF (SCKF) is proposed to resolve this problem. The filter is designed assuming that it is applied to a system of which dynamic motion is slow and not complex. The computation amount of the filter is similar to that of the EKF and its performance has similar characteristics with that of the CKF.
In this paper, this SCKF is applied to shipboard INS transfer alignment. The main system is an inertial navigation system (INS) mounted in the underwater vehicle (Slave INS (SINS)) while the sub-system is an INS mounted in the mother ship (Master INS (MINS)). An inertial measurement unit (IMU) that produces an output at 100 Hz is used in the SINS, and in the MINS, navigation information is provided to the SINS at 2 Hz. Here, the items to be considered additionally are lever-arm according to spatial separation between two systems, flexural motion of the mother ship, and time delay of measurement according to asynchronous time of two systems (Lyou & Lim 2009, Cao et al. 2017). Considering the above operation environments, a filter that performs SINS transfer alignment is designed based on the SCKF. To verify the performance of the proposed filter, Matlab-based Monte-Carlo simulations are conducted.
This paper is organized as follows: In Section 2, the SCKF is described, and in Section 3, a transfer alignment system is discussed. In Section 4, modified SCKF-based transfer alignment filter is designed, and in Section 5, the performance of the proposed filter is validated through simulation analysis. Finally, conclusions are made in the last section.
2. SIMPLIFIED CUBATURE KALMAN FILTER
First, a non-linear system as presented in Eq. (1) is considered.
\(x_k=f(x_{k-1})+w_{k-1}\)
\(z_k=h(x_k)+v_k\) (1)
where \(x_k\in\mathfrak{R}^{n_x}\) and \(z_k\in\mathfrak{R}^{n_z}\) refer to state variable and measurement, respectively, and \(f( )\) and \(h( )\) refer to system and measurement functions, respectively. In addition, \(w\) and \(v\) exhibit process noise and measurement noise, respectively, which are assumed as white noise that has the Gaussian distribution.
When the Bayesian filter is applied to nonlinear systems, it is necessary to have integration calculation such as Eq. (2) during the state variable time propagation process (Arasaratnam & Haykin 2009).
\({\hat{x}}_k^-=\int_{\mathfrak{R}^{n_x}}{f(x_{k-1})p(\left.x_{k-1}\right|Z_{k-1})dx_{k-1}}\) (2)
where \(Z_{k-1}={z_1,z_2,\cdots,z_{k-1}}\) refers to a measurement.
Furthermore, the above integral computation problem of {nonlinear function × probability density} occurs in many calculation equations required during the measurement update process and error co-variance time propagation, and these integral problems are not easy to be solved in this multivariate system. To solve this problem, two processes are needed. First, a probability distribution is all assumed as the Gaussian distribution. Next, variables in the Cartesian coordinate system are converted into those in the spherical-radial coordinate system, and the integral equation is simplified into a problem of sum using the spherical-radial rule. When the three-degree spherical-radial rule is used, Eqs. (2) and (3) can be expressed as follows (Jia et al. 2013).
\({\hat{x}}_k^-=\int_{\mathfrak{R}^{n_x}}{f(x_{k-1})N(x_{k-1};{\hat{x}}_{k-1},P_{k-1})dx_{k-1}}\)
\(=\int_{\mathfrak{R}^{n_x}}{f(S_{k-1}x_{k-1}+{\hat{x}}_{k-1})N(x_{k-1};0,I)dx_{k-1}}\)
\(\cong\sum_{i=1}^{N_c}{w_if(S_{k-1}\gamma_i+{\hat{x}}_{k-1})}\) (3)
where \(N(x;\hat{x},P)\) refers to the Gaussian distribution of which mean is \(\hat{x}\) and variance is \(P=SS^T\), and \(S\) is calculated through the Cholesky decomposition. \(\gamma_i\) and \(w_i\) are calculated by \(\sqrt{N_c/2}[1]i\) and \(1/N_c\), respectively, where \([1]i\) refers to the \(i\)-th column vector in the unit matrix, and \(N_c\) refers to the number of CPs, which is set to \(2n_x\) in the three-degree CKF.
The CKF is run according to the process flow chart shown in Fig. 1a using a CP set \({\xi_i,w_i}\) consisting of \(N_c\) CPs \(\xi_{i,k-1}=S_{k-1}\gamma_i+{\hat{x}}_{k-1}\) and weight \(w_i\).
The time propagation of the CP is conducted as presented in Eq. (4) while being synchronized with the output rate of the main system.
\(\xi_{i,k+1}^-=f(\xi_{i,k}^-)\) (4)
This process is performed \(N_c\) times iteratively. Using this information, a state variable is calculated via Eq. (5).
\({\hat{x}}_{k+1}^-=\frac{1}{N_c}\sum_{i=1}^{N_c}\xi_{i,k+1}^-\) (5)
This process is run in accordance with the output cycle of the main system.
Measurement update is performed once measurement (\(Z_{k+H}\)) is obtained from the sub-system over the time \(t_{k+H}\). To do this, time-propagated measurement estimate is calculated first.
\({\hat{z}}_{i,k+H}^-=\frac{1}{N_c}\sum_{i=1}^{N_c}{h(\xi_{i,k+H}^-)}\) (6)
Next, measurement update of state variable and error co-variance is performed.
\({\hat{x}}_{k+H}={\hat{x}}_{k+H}^-+K_{k+H}(Z_{k+H}-{\hat{Z}}_{k+H}^-)\) (7)
\(P_{k+H}=P_{k+H}^--K_{k+H}P_{zz,k+H}^-K_{k+H}^T\) (8)
where
\(K_{k+H}=P_{xz,k+H}^-(P_{zz,k+H}^-)^{-1}\) (9)
\(P_{xz,k+H}^-=\frac{1}{N_c}\sum_{i=1}^{N_c}{\xi_{i,k+H}^-(h(\xi_{i,k+H}^-))^T-}{\hat{x}}_{k+H}^-({\hat{Z}}_{k+H}^-)^T\) (10)
\(P_{zz,k+H}^-=\frac{1}{N_c}\sum_{i=1}^{N_c}{h(\xi_{i,k+H}^-)(h(\xi_{i,k+H}^-))^T}-{\hat{Z}}_{k+H}^-({\hat{Z}}_{k+H}^-)^T+R \) (11)
\(P_{k+H}^-=\frac{1}{N_c}\sum_{i=1}^{N_c}{\xi_{i,k+H}^-(\xi_{i,k+H}^-)^T}-{\hat{x}}_{k+H}^-({\hat{x}}_{k+H}^-)^T+Q\) (12)
where \(R\) and \(Q\) refer to noise co-variances of measurement and process, respectively.
CP (\(\xi_{i,k+H}=S_{k+H}\gamma_i+{\hat{x}}_{k+H}\)) is then updated again using the measurement-updated state variable and error co-variance matrix.
Eq. (4) is performed \(N_c\) times, in which a large computation load occurs in this process if the system degree is high and non-linear system function is complex. The SCKF is proposed as shown in Fig. 1b in this paper to address this problem.
Fig. 1. Processing structures of two filters. (a) CKF (b) SCKF
The concept of the SCKF is two-fold as follows: First, a state variable calculated through time propagation of CPs at the CKF is not different from that calculated through time propagation of only a single mean value as done in the EKF when nonlinearity of system function is not so large. Second, co-variance matrix, calculated by time propagation of CPs during the output cycle of the sub-system while being synchronized with the output rate of the main system in the CKF, is not so different from the co-variance matrix calculated after time propagation of CP once in accordance with the output cycle of the sub-system when the system has no sudden dynamic motions. If the SCKF is designed according to the above two conditions, it can be expressed as follows:
The time propagation of the state variable is conducted while being synchronized with the output rate of the main system.
\({\hat{x}}_{k+1}^-=f({\hat{x}}_k^-)\) (13)
Measurement update is performed once measurement is obtained from the sub-system over the time \(t_{k+H}\). To do this, time propagation of CP is conducted at the measurement update cycle.
\(ξi,k+H-=f(ξi,k-)\) (14)
The employed output data in the main system is a mean calculated during the measurement update period. The error co-variance matrix is calculated using Eq. (14). A new CP (\(\xi_{i,k+H}^-\)) is calculated by using the state variable obtained through Eq. (13) with the error co-variance matrix. Next, the measurement update of error co-variance matrix and state variable is performed through Eqs. (7) and (8) as same as in the CKF, and finally, the CP is updated.
The computation load is reduced by simplifying the time propagation process of CPs, which accounts for a large proportion of calculation process occurred in the CKF. However, the SCKF retains the strengths of the CKF by using the same mode of time propagation of error co-variance as it employs nonlinear functions as same as the one used in the CKF, which is the advantage of the CKF over the EKF.
In Section 3, the shipboard INS transfer alignment where the SCKF is applied is explained and the transfer alignment filter is designed with the SCKF.
3. CONSIDERATIONS FOR SHIPBOARD INS TRANSFER ALIGNMENT
The shipboard INS transfer alignment refers to performing initial alignment of SINS with INS-based navigation device (SINS) of underwater vehicle as a main system, and INS-based navigation device (MINS) of mother ship as a sub-system. The following should be done first for the transfer alignment: (1) Measurement of the filter should be determined through navigation information provided by the MINS, and (2) measurement update cycle should be determined through output rate that provides navigation information of the MINS. Additionally, the following issues should be taken into consideration to perform an accurate transfer alignment: (3.1) lever-arm according to spatial separation between MINS and SINS, (3.2) problem due to flexural motion in the long mother ship, and (3.3) time synchronization problem with the SINS according to time delay of navigation information provided by the MINS.
Next, the system structure of the shipboard transfer alignment is shown in Fig. 2. It is assumed that the MINS is located in the center of the naval ship, and the SINS is mounted toward the lower side downwardly at 15° from the front 50 m away from the MINS.
Fig. 2. System structure of the ship transfer alignment.
The basic state variable which will be estimated in the filter is configured with the 12th degree as presented in Eq. (15).
\(x=\left[\begin{matrix}(V^n)^T&(E^b)^T&(\nabla^b)^T&(\varepsilon^b)^T\\\end{matrix}\right]^T\) (15)
where \( V^n=[v_n~~~v_e~~~v_d]^T\) refers to velocity information in the navigation coordinate system, \(E^b=[\phi~~~\phi~~~\psi]^T\) refers to the Euler angle, and \(\nabla^b\) and \(\varepsilon^b\) refer to biases of accelerometer and gyroscope, respectively.
The first thing to be determined is the information that can be used as measurements of the filter such as a velocity, attitude, angular velocity, and acceleration information of the MINS. If velocity and attitude information is employed separately, all state variables cannot be observable as shown in Fig. 3. If only velocity information is employed (dashed line), attitude, a bias of accelerometer in the horizontal axis, and a bias of gyroscope in the vertical axis are difficult to be estimated. If only attitude information is used (dotted line), estimations of both of velocity and bias of accelerometer are not observable. In contrast, when both velocity and attitude information are employed as measurements (solid line), all 12 state variables are observable. Thus, this paper employs both velocity and attitude information basically. Although a convergence velocity of the filter is somewhat improved by using additional measurements of angular velocity and acceleration information, the complexity of the filter design is significantly increased due to the use of the additional measurements. Thus, those information is not used in this paper.
Fig. 3. Covariance analysis according to the measurements.
The second thing to be determined is a measurement update cycle, which is analyzed through Fig. 4. Basically, as the measurement update rate increases, the convergence velocity of the filter increases as well. In particular, an estimation velocity of gyroscope bias is increased, and a convergence velocity at 20 Hz of measurement update was about twice faster than that at 1 Hz (Groves 2003). Thus, it can be seen that measurements at a high output rate are needed to be provided from the MINS for rapid transfer alignment according to specific applications. However, this study, which is performed at a fixed hardware environment, assumes that measurements are provided at 2 Hz to 10 Hz.
Fig. 4. Convergence speed according to the measurement update rate.
The first thing to be considered additionally among three considerations is the lever-arm effect, which makes rotation of mother ship due to the spatial separation between MINS and SINS caused by a difference in velocity between two systems. This should be compensated by the following equation (Seo et al. 2006).
\(V_S^n=V_M^n+C_M^n(\omega_{nM}^M\times r_0)\) (16)
where \(V_S^n\) and \(V_M^n\) refer to velocity information of SINS and MINS, \(C_M^n\) respectively, refers to a convert direction cosine matrix (DCM) from the body frame coordinate system to the navigation coordinate system, \(\omega_{nM}^M\) refers to a rotational angular velocity of the MINS, and \(r_0\) refers to a lever-arm vector, which is a relative position vector of the SINS represented in the body frame coordinate system of the MINS assuming that a mother ship would be a rigid body.
The additional second consideration is a mismatch problem of velocity and attitude measurements between two systems due to flexural motion of mother ship, which must be resolved for accurate transfer alignment. The deformation of lever-arm vector due to flexural motion in each axis of the mother ship should be considered as presented below.
\(r=r_0-(\frac{1}{2}r_0\times)\Theta\) (17)
where \(\Theta=[\begin{matrix}\theta_x&\theta_y&\theta_z\\\end{matrix}]^T\) refers to a vector form of flexural angle in three axes of the mother ship. Accordingly, Eq. (15) should be modified as follows (Savage 2015):
\(V_S^n=V_M^n+C_M^n(\omega_{nM}^M\times r+\dot{r})\) (18)
where \(r\) is equivalent to Eq. (17), and \(r\) is equal to \(-\left(\frac{1}{2}r_0\times\right)\dot{\Theta}\).
Since the mother ship has a long shape in the x axis (front direction of the mother ship), flexural motion occurs mainly in the y axis (lateral direction in the right side of the mother ship). As a result, measurements that correspond to the y-axis of the MINS create a significant problem in matching with the SINS. Thus, a method that excludes the above measurement information from the measurement has been used. However, this study attempts to perform estimation by adding a flexure angle as a state variable assuming that flexure parameters of the mother ship are already known. In order to do this, flexural motion can be designed as the second-degree Markov model as follows (Lyou & Lim 2009):
\(\ddot{\Theta}+\alpha\dot{\Theta}+\beta\Theta=\eta\) (19)
where it satisfies \(\alpha=2\varsigma w_n, \beta=w_n^2\), and \(\eta~N(0,\sigma^2)\). \(\varsigma\), \(w_n\), and \(\sigma\) refer to Markov model coefficients, which can be acquired through pre-test in the case of specific mother ships.
An equation for attitude matching considering flexural motion can be expressed as follows:
\({\hat{C}}_M^n\cong{\hat{C}}_S^nC_{M^\prime}^S{\hat{C}}_M^{M^\prime}\) (20)
where \(C_S^n\) refers to a convert DCM from the body frame coordinate system to the navigation coordinate system in the SINS, \({\hat{C}}_M^{M^\prime}\)represents a flexure angle in the form of DCM, \(C_{M^\prime}^S\) represents non-alignment of attitude between the body frame coordinate system of the MINS deformed by flexural motion and the SINS in the form of DCM, which can be replaced with \(C_M^S\) and employed as it is calculated in advance.
The additional and final consideration is an asynchronous clock problem between MINS and SINS, which occurs due to a data time delay of the MINS. As shown in Fig. 4, velocity information transferred from the MINS at time \(t_{k-1+a}\) is received at time \(t_{k-1+b}\) in the SINS. Since matching is done at time \(t_k\), velocity information of the SINS should be matched after compensating it to a time that created information of the MINS as follows: Here, time delay \(\delta t_d\) is defined as shown in Fig. 5, and the compensation is done as follows:
Fig. 5. Definition of the time delay.
\(V_{S,k-\delta t_d}^n=V_{S,k}^n-(V_{S,k}^n-V_{S,k-1}^n)\frac{\delta t_d}{dt}\) (21)
where \(dt\) refers to an output cycle of the IMU data in the SINS, and time delay \(\delta t_d\) should be employed via estimation after it is added as a state variable.
Similarly, the attitude information of the SINS should be employed after it is compensated as follows:
\(E_{S,k-\delta t_d}^b=E_{S,k}^b-(E_{S,k}^b-E_{S,k-1}^b)\frac{\delta t_d}{dt}\) (22)
The time delay compensation is performed using the Euler angle of the SINS and MINS followed by changing it to DCM to perform attitude matching.
4. MODIFIED SCKF-BASED TRANSFER ALIGNMENT FILTER DESIGN
The five considerations mentioned in Section 3 are considered to design the SCKF-based transfer alignment filter as follows: The state variables consist of the 19th degree including time delay, flexural motion angle, and angle change rate in addition to the 12th degree as shown in Eq. (5).
\(x=\left[\begin{matrix}(V^n)^T&(E^b)^T&(\nabla^b)^T&(\varepsilon^b)^T&\delta t_d&\Theta^T&{\dot{\Theta}}^T\\\end{matrix}\right]^T\) (23)
The structure shown in Fig. 1b and time delay compensation equations: Eqs. (21) and (22) should be considered together to update measurements and time propagation. Since values of the previous state variables should be employed together at the time propagation interval in order to compensate time delay, a structure of the generalized SCKF, which is shown in Fig. 1b, shall be modified to a structure shown in Fig. 6 before using.
Fig. 6. Processing structure of the modified SCKF.
When time propagation occurs \(H\) times within the measurement update cycle, it is processed as same as done in the SCKF up to the \(H\)-1-th time and the last one is run as same as done in the CKF. Through this, a problem of time delay can be resolved using previous time CP information when measurement update is done.
A total of three cases of time propagation can be done. First, time propagation occurs from time \(t_k\) to \(t_{k+H-1}\) with a cycle of \(dt\). Second, time propagation of CP occurs one at a time in the same time interval with a cycle of \((H-1)dt\). Third, time propagation of CP occurs one at a time from time \(t_{k+H-1}\) to \(t_{k+H}\) with a cycle of \(dt\).
A function used in time propagation is a differential equation of velocity and attitude calculation in the INS. Among three time propagations, the third time propagation equation is summarized as follows: Time propagation of attitude information is performed by using the quaternion differential equation as follows:
\(\dot{q}(i)=\frac{1}{2}q(i)\ast\left(\omega_{ib}^b-\xi_{10:12}^-(i)-C_n^b(i)(\omega_{ie}^n+\omega_{en}^n)\right)\) (24)
where \(\omega_{ib}^b\) refers to an output of gyroscope, and \(\xi_{a:b}^-(i)\) refers to the \(a–b\)-th components of the i-th CP. That is, Eq. (23) indicates that 1:3, 4:6, 7:9, 10:12, 13, 14:16, and 17:19 represent velocity, Euler angle, bias of accelerometer, bias of gyroscope, time delay, flexural motion angle and angle change rate of the hull, respectively. \(q(i)\) refers to the quaternions. CP \((\xi_{4:6}^-(i))\) that corresponds to the Euler angle is changed into the quaternions and then time propagation is performed through Eq. (24). Then, the result is changed back into \( \xi_{4:6}^-(i)\) again to complete time propagation of attitude information. Here, \(i=1,2,\cdots,N_c\).
Time propagation of velocity information can be done as follows:
\({\dot{\xi}}_{1:3}^-(i)=C_b^n\left(f^b-\xi_{7:9}^-(i)\right)-(2\omega_{ie}^n+\omega_{en}^n)\times\xi_{1:3}^-(i)+g^n\) (25)
where \(f^b\) refers to an output of accelerometer, and \(g^n\) refers to a gravitational acceleration vector.
Furthermore, time propagation of hull flexural motion-related CP is done as follows:
\({\dot{\xi}}_{17:19}^-(i)=-\alpha\xi_{14:16}^-(i)-\beta\xi_{17:19}^-(i)\)
\({\dot{\xi}}_{14:16}^-(i)=\xi_{17:19}^-(i)\) (26)
When differential equations of Eqs. (24) and (26) are solved, the cycle is \(dt\).
Once a measurement is inputted from the MINS, an estimate of the measurement is generated first to update the measurement. The estimates of the measurement for velocity and attitude matching are as follows:
\({\hat{z}}_{k+H}^-=\left[\begin{matrix}{\hat{V}}_{M,k+H}^-\\E_{M,k+H}^{b-}\\\end{matrix}\right]\) (27)
where \({\hat{V}}_{M,k+H}^-\) and \(E_{M,k+H}^{b-}\) refer to velocity and Euler angle information of the MINS calculated using the SINS information, respectively, which can be calculated as follows considering the time delay and lever-arm, as well as flexural hull motion.
\({\hat{V}}_{M,k+H}^-=\frac{1}{N_c}\sum_{i=1}^{N_c}\left({\hat{V}}_{S,k+H}^-(i)-C_{M,k}^n(i)(\omega_{nb}^b(i)\times\hat{r}(i)+\widehat{\dot{r}}(i))\right)\) (28)
where
\({\hat{V}}_{S,k+H}^-=\xi_{1:3,k+H}^-(i)-\left(\xi_{1:3,k+H}^-(i)-\xi_{1:3,k+H-1}^-(i)\right)\frac{\xi_{13,k+H}^-(i)}{dt}\) (29)
\(\hat{r}(i)=r_0-\frac{1}{2}r_0\times\xi_{14:16,k+H}^-(i)\) (30)
\(\widehat{\dot{r}}(i)=-\frac{1}{2}r_0\times\xi_{17:19,k+H}^-(i)\) (31)
The calculation of \(E_{M,k+H}^{b-}\) is as follows:
\(E_{M,k+H}^{b-}=\frac{1}{N_c}\sum_{i=1}^{N_c}\left(DCM2Euler\left({\hat{C}}_{M,k+H}^{n-}(i)\right)\right)\) (32)
where \(DCM2Euler()\) refers to a function that converts DCM into Euler angle (Siouris 1993).
\({\hat{C}}_{M,k+H}^{n-}(i)={\hat{C}}_{S,k+H}^{n-}C_M^S\cdot Euler2DCM\left(\xi_{14:16,k+H}^-(i)\right)\) (33)
where \(Euler2DCM()\) refers to a function that converts Euler angle into DCM (Siouris 1993).
\({\hat{C}}_{S,k+H}^{n-}=Euler2DCM\left({\hat{E}}_{S,k+H}^-(i)\right)\) (34)
\({\hat{E}}_{S,k+H}^-=\xi_{4:6,k+H}^-(i)-\left(\xi_{4:6,k+H}^-(i)-\xi_{4:6,k+H-1}^-(i)\right)\frac{\xi_{13,k+H}^-(i)}{dt}\) (35)
The measurement update of state variable and error co-variance matrix can be done as presented in Eqs. (6) and (7), respectively.
5. SIMULATION ANALYSIS
In order to validate the proposed SCKF and its performance of the shipboard INS transfer alignment filter, simulations were conducted. The simulation environment is set as follows:
• The SINS mounted underwater vehicle is located at 50 m front from the MINS of the mother ship in the forward direction (x axis).
• The underwater vehicle is positioned having the Euler angles (0°, -15°, 0°) from the MINS.
• The mother ship is accelerated in the north direction for 10 sec. and operated at a velocity of 10 m/s.
• Waves are designed with the second-degree Markov process.
• The specifications of the SINS sensor are presented in Table 1.
• Asynchronous time difference between MINS and SINS is 3 ms.
• The Markov model coefficient of flexural hull motion is presented in Table 2.
• The output rate of providing MINS information is assumed as two: 10 Hz and 2 Hz.
• One-shot alignment is performed to determine the initial velocity and attitude of the SINS using the MINS information prior to transfer alignment.
Table 1. Spec. of SINS sensors.
| Sensor | Error | Spec. |
| Accelerometer Gyro |
Bias repeatability Velocity random walk Bias repeatability Angular random walk |
3.5 mg 0.085 mg/√hr 20 deg/hr 0.02 deg/√hr |
Table 2. Markov model parameters for the flexure motion.
| Axis | \(w_n\) (Hz) | \(\varsigma\) | \(\sigma\) (deg) |
| X Y Z |
0.20 0.15 0.25 |
0.1 0.5 0.5 |
0.01 0.1 0.001 |
Fig. 7. Transfer alignment simulation results – covariance analysis.
Fig. 7 shows the results after performing co-variance analysis. From the vertical axis, velocity, attitude, bias of accelerometer, bias of gyroscope, time delay, and flexural motion angle are shown in order. Once both velocity and attitude information are utilized as measurements, observability of the basic 12th degree model is satisfied, and time delay and flexural motion angle, which are previously designed, can also be observable additionally. Thus, it verifies that observability of the basic filter is all satisfied.
Figs. 8a and 8b show the estimation results of state variables when measurement update rate is 2 Hz and 10 Hz. The black-colored dotted line represents a true value, and blue, red, and black-colored solid lines represent the results of EKF, CKF, and SCKF, respectively. The fluctuating results of position and velocity are due to the wave, which is normal. The figures indicate that biases of the accelerometer and gyroscope are converged to the true value as measurements are updated. The reason for the relatively slow convergence rate of gyroscope bias can be found through the co-variance analysis, which is exhibited in Fig. 7. As for the time delay, estimation occurs immediately at the early time of the measurement update. The x- and y-axes in the flexural hull motion angle gradually show better tracking of the flexural hull motion. Although the z-axis does not seem to converge well, its flexural motion size is significantly smaller than those of the x- and y-axes, thus, it is insignificant. As the most important y-axis flexural motion is well tracked, it is found that the degradation in performance of transfer alignment due to flexural hull motion is well resolved.
Fig. 8. Transfer alignment simulation results – state estimates. (a) measurement update rate: 2 Hz (b) measurement update rate: 10 Hz
The reason for the similar estimation performance between EKF and CKF is due to the small initial estimation error of the SINS through the one-shot alignment prior to the transfer alignment. Furthermore, the performance of the SCKF designed in this paper can be validated by verifying no difference in results between SCKF and CKF.
Fig. 9 shows the results of computation amount in each filter using Matlab's tic/toc command. This figure displays a processing time of computation in each filter when the measurement update rate is 2 Hz. A large computation is required during the time propagation process in CKF compared to that of EKF. The reason for this is to perform the same calculation times iteratively. Because of this, a real-time implementation of CKF in embedded systems can be somewhat difficult. On the contrary, a computation load for time propagation in the SCKF is similar to that of the EKF, and a computation load for measurement update is similar to that of the CKF. Thus, an overall computation load is significantly lower than that of CKF, and similar to that of the EKF as verified in the figure.
Finally, Fig. 10 displays the Monte-Carlo simulation results of the modified SCKF. The red color indicates means of simulation results after 30 times of executions, and the blue color represents (mean + 1σ) and (mean - 1σ). The analysis results between Fig. 7 and Fig. 10 show that transfer alignment designed based on the modified SCKF is normally executed.
Fig. 9. Processing times of the filters.
Fig. 10. Transfer alignment simulation results – Monte-Carlo simulation.
The SCKF proposed in this paper and the modified SCKF, which is applied to transfer alignment, aims to reduce a computation load of CKF rather than aiming for improvements on performance. Furthermore, the reason for the application of CKF, which did not show improved performance compared to that of EKF, to transfer alignment of which initial state variable error was not so large was due to the easy application of the filter. For EKF, the Jacobian matrix has to be configured by inducing an error model of state variable, and it is not easy to induce an error model when a new state variable is added. In contrast, CKF can be easily employed if a relationship between state variables during the update process using the time propagation and measurement equation that uses the given system differential equation is represented as explained in Section 4. The effectiveness of the filter is improved by reducing the increasing computation amount using the SCKF.
Thus, the SCKF designed in this paper has significantly lower computation load compared to that of CKF, but its performance is comparable to that of CKF, which is verified through simulations. Accordingly, the proposed filter can be utilized usefully in real-time implementation conclusively.
6. CONCLUSIONS
In this paper, the SCKF that reduced a computation load without degradation in performance of CKF was proposed and the filter was utilized in shipboard INS transfer alignment. The CKF requires much larger computation than the EKF during the time propagation process in a fusion system in which filter's time propagation and measurement update cycle are different, and its computation load becomes larger as the system's degree becomes higher. This paper resolved this problem by designing the SCKF, in which time propagation of state variable in the EKF and co-variance time propagation in the CKF were fused, considering a mother ship whose dynamic motion was slow and not complex. The proposed filter was utilized in the transfer alignment filter design of shipboard INS to verify the performance. The transfer alignment filter design took an accurate transfer alignment system into consideration. In order to do this, the concepts of measurement type and selection of measurement update cycle were summarized, and a transfer alignment filter was designed in consideration of lever-arm, flexural hull motion, and data time delay between SINS and MINS additionally. Simulations were conducted to verify the performance of the filter. The simulation results showed that the performance of the proposed SCKF was comparable with that of CKF, and its computation amount became as low as that of EKF. The above study result verified that the real-time implementation of CKF can be achieved and the utilization of the SCKF can be increased further by applying to the 5th degree or higher CKF in the future.
ACKNOWLEDGMENTS
This work was supported by Agency for Defense Development [UD150004DD]
References
- Arasaratnam, I. & Haykin, S. 2009, Cubature Kalman filters, IEEE Trans. Automatic Control, 54, 1254-1269. https://doi.org/10.1109/TAC.2009.2019800
- Cao, Q., Zhong, M., & Guo, J. 2017, Non-linear Estimation of the Flexural Lever Arm for Transfer Alignment of Airborne Distributed Position and Orientation System, IET Radar, Sonar & Navigation, 11, 41-51. https://doi.org/10.1049/iet-rsn.2015.0541
- Gelb, A. 1974, Applied Optimal Estimation (Cambridge, MA: The MIT Press)
- Groves, P. D. 2003, Optimising the Transfer Alignment of Weapon INS, Journal of Navigation, 56, 323-335. https://doi.org/10.1017/S0373463303002261
- Jia, B., Xin, M., & Cheng, Y. 2013, High-degree Cubature Kalman Filter, Automatica, 49, 510-518. https://doi.org/10.1016/j.automatica.2012.11.014
- Julier, S. J., Uhlmann, J. K., & Durrant-Whyte, H. F. 2000, A New Method for Nonlinear Transformation of Means and Covariances in Filters and Estimators, IEEE Trans. on Automatic Contol, 45, 472-482. https://doi.org/10.1109/9.847726
- Lyou, J. & Lim, Y. C. 2009, Transfer Alignment Considering Measuremnt Time Delay and Ship Body Flexure, Journal of Mechanical Science and Technology, 23, 195-203. https://doi.org/10.1007/s12206-008-0821-y
- Maybeck, P. S. 1979, Stochastic Models, Estimation, and Control, vol.1 (NY: Academic Press)
- Ristic, B., Arulampalam, S., & Gordon, N. 2004, Beyond the Kalman Filter (Boston: Artech House)
- Savage, P. G. 2015, Lever Arm Corrections During INS Transfer Alignment with Wide Angle Initial Heading Error, SAI-WBN-14008, Starpdown Associates, Inc., free access available at http://www.strapdownassociates.com
- Seo, J., Lee, H. K., Lee, J. G., & Park, C. G. 2006, Lever Arm Compensation for GPS/INS/Odometer Integrated System, IJCAS, 4, 247-254.
- Siouris, G. M. 1993, Aerospace Avionics Systems (San Siego: Academic Press)