Skip to main content
SpringerLink
Log in

Asynchronous direct Kalman filtering approach for underwater integrated navigation system

  • Original Paper
  • Published:
Nonlinear Dynamics Aims and scope Submit manuscript

Abstract

This paper presents an asynchronous direct Kalman filter (ADKF) approach for underwater integrated navigation system to improve the performance of the prevalent indirect Kalman filter structure. The designed navigation system is composed of a strapdown inertial navigation system (SDINS) along with Doppler Velocity Log, inclinometer, and depthmeter. In the proposed approach, prediction procedure is placed in the SDINS loop and the correction procedure operates asynchronously out of the SDINS loop. In contrast to the indirect Kalman filter, in the ADKF, the total state such as position, velocity, and orientation are estimated directly within the filter, and further calculations are not performed outside of the filter. This reduces the running time of the computations. To the best of our knowledge, no results have been reported in the literature on the experimental evaluation of a direct Kalman filtering for underwater vehicle navigation. The performance of the designed system is studied using real measurements. The results of the lake test show that the running time of the proposed approach can be improved approximately 7.5 % and also the ADKF exhibits an average improvement of almost 20 % in position estimate with respect to the prevalent indirect Kalman filter.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9

Similar content being viewed by others

References

  1. Brokloff, N.: Dead reckoning with an adcp and current extrapolation. In: IEEE/MTS OCEANS, vol. 2, pp. 994–100. Halifax, NS, Canada (October 1997)

  2. Groves, P.D.: Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House, Boston (2008)

    MATH  Google Scholar 

  3. Park, M., Gao, Y.: Error analysis and stochastic modeling of low-cost mems accelerometer. J. Intell. Robot. Syst. 46(1), 27–41 (2006)

    Article  Google Scholar 

  4. Farrell, J.A.: Aided Navigation GPS with High Rate Sensors. McGraw-Hill Professional, New York (2008)

    Google Scholar 

  5. RD Instruments: ADCP Coordinate Transformation, Formulas and Calculations. San Diego, RD Instruments (1998)

  6. Jalving, B., Gade, K., Hagen, O.K.: A toolbox of aiding techniques for the hugin auv integrated inertial navigation system. In: Oceans 2003 MTS/IEEE, San Diego, CA (September 2003)

  7. Pitman, G.R.: Inertial Guidance. Wiley, New York (1962)

    Book  MATH  Google Scholar 

  8. Britting, K.R.: Inertial Navigation Systems Analysis. Wiley, New York (1971)

    Google Scholar 

  9. Ali, J., Ullah, R., Mirza, M.B.: Performance comparison among some nonlinear lters for a low cost sins/gps integrated solution. Nonlinear Dyn. 61(3), 491–502 (2010)

    Article  MATH  Google Scholar 

  10. Jwo, D.J., Yang, C.F., Chuang, C.H., Lee, T.Y.: Performance enhancement for ultra-tight gps/ins integration using a fuzzy adaptive strong tracking unscented kalman lter. Nonlinear Dyn. 73(1–2), 377–395 (2013)

    Article  MathSciNet  Google Scholar 

  11. Ali, J., Ushaq, M., Majeed, S.: Robust aspects in dependable lter design for alignment of a submarine inertial navigation system. J. Mar. Sci. Technol. 17(3), 340–348 (2012)

    Article  Google Scholar 

  12. Yun, X., Bachmann, E.R., McGhee, R.B., Whalen, R.H.: Testing and evaluation of an integrated gps/ins system for small auv navigation. IEEE J. Ocean. Eng. 24(3), 396–404 (1999)

    Article  Google Scholar 

  13. Larsen, M.: High performance doppler-inertial navigation—experimental results. In: Proceedings of OCEANS 2000 MTS/IEEE, Rhode Island Convention Center (September 2000)

  14. McEwen, R., Thomas, H., Weber, D., Psota, F.: Performance of an AUV navigation system at arctic latitudes. IEEE J. Ocean. Eng. 30(2), 443–454 (2005)

    Article  Google Scholar 

  15. Kussat, N.H., Chadwell, C.D., Zimmerman, R.: Absolute positioning of an autonomous underwater vehicle using gps and acoustic measurements. IEEE J. Ocean. Eng. 30(1), 153–164 (2005)

    Article  Google Scholar 

  16. Kinsey, J.C., Eustice, R.M., Whitcomb, L.L.: A survay of underwater vehicle navigation: Recent advances and new challenges. In: IFAC Conference on Maneuvering and Control of Marine Craft, Lisbon, Portugal (September 2006)

  17. Lee, P.M., Jun, B.H., Kim, K., Lee, J., Aoki, T., Hyakudome, T.: Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle. IEEE J. Ocean. Eng. 32(2), 327–345 (2007)

    Article  Google Scholar 

  18. Miller, P.A., Farrell, J.A., Zhao, Y., Djapic, V.: Autonomous underwater vehicle navigation. IEEE J. Ocean. Eng. 35(3), 663–678 (2010)

    Article  Google Scholar 

  19. Vasilijevic, A., Borovic, B., Vukic, Z.: Underwater vehicle localization with complementary filter: performance analysis in the shallow water environment. J. Intell. Robot. Syst. 68(3–4), 373–386 (2012)

    Article  Google Scholar 

  20. Grenon, G., An, P.E., Smith, S.M., Healey, A.J.: Enhancement of the inertial navigation system for the morpheus autonomous underwater vehicles. IEEE J. Ocean. Eng. 26(4), 548–560 (2001)

    Article  Google Scholar 

  21. Marco, D.B., Healey, A.J.: Command, control, and navigation experimental results with the NPS ARIES AUV. IEEE J. Ocean. Eng. 26(4), 466–476 (2001)

    Article  Google Scholar 

  22. Lee, P.M., Jun, B.H.: Pseudo long base line navigation algorithm for underwater vehicles with inertial sensors and two acoustic range measurement. Ocean Eng. 34(3), 416–425 (2006)

    Google Scholar 

  23. Stutters, L., Liu, H., Tiltman, C., Brown, D.J.: Navigation technologies for autonomous underwater vehicles. IEEE Trans. Syst. Man Cybern. 38(4), 581–589 (2008)

    Article  Google Scholar 

  24. Grewal, M.S., Andrews, A.P.: Kalman Filtering Theory and Practice Using MATLAB, 3rd edn. Wiley, New Jersey (2008)

    Book  Google Scholar 

  25. Brown, R.G., Hwang, P.Y.C.: Introduction to Random Signals and Applied Kalman Filtering. Wiley, New York (1997)

    MATH  Google Scholar 

  26. Bar-Shalom, Y., Lee, X.R., Kirubarajan, T.: Estimation with Applications To Tracking and Navigation. Wiley, New York (2001)

    Book  Google Scholar 

  27. Kalman, R.E.: A new approach to linear filtering and prediction problems. ASME Trans. Ser. D: J. Basic Eng. 82(1), 34–45 (1960)

  28. Majji, M., Junkins, J.L., Turner, J.D.: A perturbation method for estimation of dynamic systems. Nonlinear Dyn. 60(3), 303–325 (2010)

  29. Maybeck, P.S.: Stochastic models, estimation, and control, vol. 1. Academic Press, New York (1979)

    MATH  Google Scholar 

  30. Farrell, J.A., Barth, M.: The Global Positioning System and Inertial Navigation. McGraw-Hill Professional, New York (1999)

    Google Scholar 

  31. Qi, H., Moore, J.B.: Direct Kalman filtering approach for GPS/INS integration. IEEE Trans. Aerosp. Electron. Syst. 38(2), 687–693 (2002)

    Article  Google Scholar 

  32. Wendel, J., Schlaile, C., Trommer, G.F.: Direct Kalman filtering of GPS/INS for aerospace applications. In: International Symposium on Kinematic System in Geodesy, Geomatics and Navigation, Alberta, Canada (2001)

  33. Zhao, L., Gao, W.: The experimental study on gps/ins/dvl integration for auv. In: Proceedings of the Position Location Navigation Symposium, PLANS 2004. pp. 337–340 (April 2004)

  34. Jo, G., Choi, H.S.: Velocity-aided underwater navigation system using receding horizon Kalman filter. IEEE J. Ocean. Eng. 31(3), 565–573 (2006)

    Article  Google Scholar 

  35. Hegrens, O., Hallingstad, O.: Model-aided INS with sea current estimation for robust underwater navigation. IEEE J. Ocean. Eng. 36(2), 316–337 (2011)

    Article  Google Scholar 

  36. Titterton, D.H., Weston, J.L.: Strapdown Inertial Navigation Technology. The Institution of Electrical Engineers, UK (2004)

    Book  Google Scholar 

  37. Simon, D.: Optimal State Estimation Kalman, \(\text{ H }\infty \) and Nonlinear Approaches. Wiley, New Jersey (2006)

    Google Scholar 

  38. Mariani, S., Ghisi, A.: Unscented kalman ltering for nonlinear structural dynamics. Nonlinear Dyn. 49(1–2), 131–150 (2007)

    Article  MATH  Google Scholar 

  39. Shabani, M., Gholami, A., Davari, N., Emami, M.: Implementation and performance comparison of indirect kalman filtering approaches for AUV integrated navigation system using low cost IMU. In: ICEE2013, Mashhad, Iran, pp. 1–6 (May 2013)

Download references

Acknowledgments

The authors would like to thank Mehdi Emami for his help in the experiments, and the anonymous reviewers for providing valuable comments to improve this paper.

Author information

Authors and Affiliations

  1. Department of ECE, Isfahan University of Technology, 84156-83111, Isfahan, Iran

    Mohammad Shabani, Asghar Gholami & Narjes Davari

Authors
  1. Mohammad Shabani
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Asghar Gholami
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Narjes Davari
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Mohammad Shabani.

Appendix: The derivatives in the Jacobian matrices \(\mathbf {F}\), \(\mathbf {G}\), and \(\mathbf {H}_\mathrm{dvl}\)

Appendix: The derivatives in the Jacobian matrices \(\mathbf {F}\), \(\mathbf {G}\), and \(\mathbf {H}_\mathrm{dvl}\)

The matrices \(\mathbf {F}\) and \(\mathbf {G}\) are formed according to the matrices at the top of the next page.

The derivatives in the Jacobian matrix \(\mathbf {F}\) are given by

$$\begin{aligned} \mathbf {F}&=\left( \begin{array}{ccccccccc} F_{11} &{} 0 &{} F_{13}&{}F_{14} &{} 0 &{} 0&{}0 &{} 0 &{} 0 \\ F_{21} &{} 0 &{} F_{23}&{}0 &{} F_{25} &{} 0&{}0 &{} 0 &{} 0\\ 0 &{} 0 &{} 0&{}0 &{} 0 &{} F_{36}&{}0 &{} 0 &{} 0\\ F_{41} &{} 0 &{} F_{43}&{}F_{44} &{} F_{45} &{} F_{46}&{}F_{47} &{} F_{48} &{} F_{49}\\ F_{51} &{} 0 &{} F_{53}&{}F_{54} &{} F_{55} &{} F_{56}&{}F_{57} &{} F_{58} &{} F_{59}\\ F_{61} &{} 0 &{} F_{63}&{}F_{64} &{} F_{65} &{} 0&{}F_{67} &{} F_{68} &{} F_{69}\\ F_{71} &{} 0 &{} F_{73}&{}F_{74} &{} F_{75} &{} F_{76}&{}F_{77} &{} F_{78} &{} F_{79}\\ F_{81} &{} 0 &{} F_{83}&{}F_{84} &{} F_{85} &{} F_{86}&{}F_{87} &{} F_{88} &{} F_{89}\\ F_{91} &{} 0 &{} F_{93}&{}F_{94} &{} F_{95} &{} F_{96}&{}F_{97} &{} F_{98} &{} F_{99}\\ \end{array} \right) ,\\ \mathbf {G}&=\left( \begin{array}{cccccc} 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0 \\ C_{11} &{} C_{12} &{} C_{13} &{} 0 &{} 0 &{} 0\\ C_{21} &{} C_{22} &{} C_{23} &{} 0 &{} 0 &{} 0\\ C_{31} &{} C_{32} &{} C_{33} &{} 0 &{} 0 &{} 0\\ 0 &{} 0 &{} 0 &{} 1 &{} \sin \phi \tan \theta &{} \cos \phi \tan \theta \\ 0 &{} 0 &{} 0 &{} 0 &{} \cos \phi &{} -\sin \phi \\ 0 &{} 0 &{} 0 &{} 0 &{} \sin \phi \sec \theta &{} \cos \phi \sec \theta \\ \end{array} \right) \end{aligned}$$
$$\begin{aligned} F_{11}&=\frac{\partial f_1}{\partial L}=-\frac{\partial R_N}{\partial L}\frac{v_N}{R_N+Z}\\ F_{13}&=\frac{\partial f_1}{\partial Z}=-\frac{v_N}{(R_N+Z)^2}\\ F_{14}&=\frac{\partial f_1}{\partial v_N}=\frac{1}{R_N+Z}\\ F_{21}&=\frac{\partial f_2}{\partial L}=v_E\left[ \frac{\sec L \tan L}{R_E+Z}+\frac{-\partial R_N}{\partial L}\frac{\sec L}{(R_E+Z)^2}\right] \\ F_{23}&=\frac{\partial f_2}{\partial Z}=-\frac{v_E \sec L}{(R_E+Z)^2}\\ F_{25}&=\frac{\partial f_2}{\partial v_E}=\frac{\sec L}{R_E+Z}\\ F_{36}&=\frac{\partial f_3}{\partial v_D}=1\\ F_{41}&=\frac{\partial f_4}{\partial L}=-v_E\left[ \frac{\partial f_2}{\partial L}\sin L+\frac{v_E}{R_E+Z}\right] +v_D\frac{\partial f_1}{\partial L}\\ F_{43}&=\frac{\partial f_4}{\partial Z}=-v_E\left( \sin L \frac{\partial f_2}{\partial Z}\right) +v_D\frac{\partial f_1}{\partial Z} \\ F_{44}&=\frac{\partial f_4}{\partial v_N}=v_D\frac{\partial f_1}{\partial v_N}\\ F_{45}&=\frac{\partial f_4}{\partial v_E}=-2\varOmega \sin L - \frac{2v_E \tan l}{R_E+Z}\\ F_{46}&=\frac{\partial f_4}{\partial v_D}=\frac{v_N}{R_N+Z}\\ F_{47}&=\frac{\partial f_4}{\partial \phi }=\frac{\partial C_{11}}{\partial \phi }f_x+\frac{\partial C_{12}}{\partial \phi }f_y+\frac{\partial C_{13}}{\partial \phi }f_z \end{aligned}$$

and

$$\begin{aligned} F_{48}&=\frac{\partial f_4}{\partial \theta }=\frac{\partial C_{11}}{\partial \theta }f_x+\frac{\partial C_{12}}{\partial \theta }f_y+\frac{\partial C_{13}}{\partial \theta }f_z\\ F_{49}&=\frac{\partial f_4}{\partial \psi }=\frac{\partial C_{11}}{\partial \psi }f_x+\frac{\partial C_{12}}{\partial \psi }f_y+\frac{\partial C_{13}}{\partial \psi }f_z\\ F_{51}&=\frac{\partial f_5}{\partial L}=v_N\left[ 2\varOmega \cos L +\frac{\partial f_2}{\partial L}\sin L+\frac{v_E \tan L}{R_E+Z} \right] \\&\qquad +v_D\left[ -2\varOmega \sin L +\frac{\partial f_2}{\partial L}\cos L+\frac{v_E}{R_E+Z} \right] \\ F_{53}&=\frac{\partial f_5}{\partial Z}=\frac{\partial f_2}{\partial Z}\left( v_N\sin L+v_D\cos L\right) \\ F_{54}&=\frac{\partial f_5}{\partial v_N}=\left( 2\varOmega +\frac{v_E\sec L}{R_E+Z}\right) \sin L\\ F_{55}&=\frac{\partial f_5}{\partial v_E}=\frac{v_D+v_N\tan L}{R_E+Z}\\ F_{56}&=\frac{\partial f_5}{\partial v_D}=2\varOmega \cos L+\frac{v_E}{R_E+Z}\\ F_{57}&=\frac{\partial f_5}{\partial \phi }=\frac{\partial C_{21}}{\partial \phi }f_x+\frac{\partial C_{22}}{\partial \phi }f_y+\frac{\partial C_{23}}{\partial \phi }f_z\\ F_{58}&=\frac{\partial f_5}{\partial \theta }=\frac{\partial C_{21}}{\partial \theta }f_x+\frac{\partial C_{22}}{\partial \theta }f_y+\frac{\partial C_{23}}{\partial \theta }f_z\\ F_{59}&=\frac{\partial f_5}{\partial \psi }=\frac{\partial C_{21}}{\partial \psi }f_x+\frac{\partial C_{22}}{\partial \psi }f_y+\frac{\partial C_{23}}{\partial \psi }f_z\\ F_{61}&=\frac{\partial f_6}{\partial L}=-v_E\left[ -2\varOmega \sin L+\frac{\partial f_2}{\partial L}\cos L\right] \\&\quad -v_E\left[ \frac{v_E\tan L}{R_E+Z}\right] -v_N \frac{\partial f_1}{\partial L}\\ F_{63}&=\frac{\partial f_6}{\partial Z}=-v_E\cos L \frac{\partial f_2}{\partial Z}-v_N \frac{\partial f_1}{\partial Z}\\ F_{64}&=\frac{\partial f_6}{\partial v_N}=\frac{-2v_N}{R_N+Z}\\ F_{65}&=\frac{\partial f_6}{\partial v_E}=-2\varOmega \cos L-\frac{2v_E}{R_E+Z}\\ F_{67}&=\frac{\partial f_6}{\partial \phi }=\frac{\partial C_{31}}{\partial \phi }f_x+\frac{\partial C_{32}}{\partial \phi }f_y+\frac{\partial C_{33}}{\partial \phi }f_z\\ F_{68}&=\frac{\partial f_6}{\partial \theta }=\frac{\partial C_{31}}{\partial \theta }f_x+\frac{\partial C_{32}}{\partial \theta }f_y+\frac{\partial C_{33}}{\partial \theta }f_z \end{aligned}$$
$$\begin{aligned} F_{69}&=\frac{\partial f_6}{\partial \psi }=\frac{\partial C_{31}}{\partial \psi }f_x+\frac{\partial C_{32}}{\partial \psi }f_y+\frac{\partial C_{33}}{\partial \psi }f_z\\ F_{71}&=\frac{\partial f_7}{\partial L}=\left( \frac{\partial \omega _y}{\partial L}\sin \phi +\frac{\partial \omega _z}{\partial L}\cos \phi \right) \tan \theta \\&\qquad +\frac{\partial \omega _x}{\partial L}\\ F_{73}&=\frac{\partial f_7}{\partial Z}=\left( \frac{\partial \omega _y}{\partial Z}\sin \phi +\frac{\partial \omega _z}{\partial Z}\cos \phi \right) \tan \theta \\&\qquad +\frac{\partial \omega _x}{\partial Z}\\ F_{74}&=\frac{\partial f_7}{\partial v_N}=\left( \frac{\partial \omega _y}{\partial v_N}\sin \phi +\frac{\partial \omega _z}{\partial v_N}\cos \phi \right) \tan \theta \\&\qquad +\frac{\partial \omega _x}{\partial v_N}\\ F_{75}&=\frac{\partial f_7}{\partial v_E}=\left( \frac{\partial \omega _y}{\partial v_E}\sin \phi +\frac{\partial \omega _z}{\partial v_E}\cos \phi \right) \tan \theta \\&\qquad +\frac{\partial \omega _x}{\partial v_E}\\ F_{76}&=\frac{\partial f_7}{\partial v_D}=\left( \frac{\partial \omega _y}{\partial v_D}\sin \phi +\frac{\partial \omega _z}{\partial v_D}\cos \phi \right) \tan \theta \\&\qquad +\frac{\partial \omega _x}{\partial v_D}\\ F_{77}&=\frac{\partial f_7}{\partial \phi }=\left( \frac{\partial \omega _y}{\partial \phi }\sin \phi +\omega _y \cos \phi \right) \tan \theta \\&\qquad +\left( \frac{\partial \omega _z}{\partial \phi }\cos \phi -\omega _z \sin \phi \right) \tan \theta +\frac{\partial \omega _x}{\partial \phi }\\ F_{78}&=\frac{\partial f_7}{\partial \theta }=\left( \frac{\partial \omega _y}{\partial \theta }\sin \phi +\frac{\partial \omega _z}{\partial \theta }\cos \phi \right) \tan \theta \\&\qquad +\left( \omega _y \sin \phi +\omega _z \cos \phi \right) \sec ^2 \theta +\frac{\partial \omega _x}{\partial \theta }\\ F_{79}&=\frac{\partial f_7}{\partial \psi }=\left( \frac{\partial \omega _y}{\partial \psi }\sin \phi +\frac{\partial \omega _z}{\partial \psi }\cos \phi \right) \tan \theta +\frac{\partial \omega _x}{\partial \psi }\\ F_{81}&=\frac{\partial f_8}{\partial L}=\left( \frac{\partial \omega _y}{\partial L}\cos \phi -\frac{\partial \omega _z}{\partial L}\sin \phi \right) \\ F_{83}&=\frac{\partial f_8}{\partial Z}=\left( \frac{\partial \omega _y}{\partial Z}\cos \phi -\frac{\partial \omega _z}{\partial Z}\sin \phi \right) \end{aligned}$$
$$\begin{aligned} F_{84}&=\frac{\partial f_8}{\partial v_N}=\left( \frac{\partial \omega _y}{\partial v_N}\cos \phi -\frac{\partial \omega _z}{\partial v_N}\sin \phi \right) \\ F_{85}&=\frac{\partial f_8}{\partial v_E}=\left( \frac{\partial \omega _y}{\partial v_E}\cos \phi -\frac{\partial \omega _z}{\partial v_E}\sin \phi \right) \\ F_{86}&=\frac{\partial f_8}{\partial v_D}=\left( \frac{\partial \omega _y}{\partial v_D}\cos \phi -\frac{\partial \omega _z}{\partial v_D}\sin \phi \right) \\ F_{87}&=\frac{\partial f_8}{\partial \phi }=\left( \frac{\partial \omega _y}{\partial \phi }\cos \phi -\omega _y \sin \phi \right) \\&\qquad -\left( \frac{\partial \omega _z}{\partial \phi }\sin \phi +\omega _z \cos \phi \right) \\ F_{88}&=\frac{\partial f_8}{\partial \theta }=\left( \frac{\partial \omega _y}{\partial \theta }\cos \phi -\frac{\partial \omega _z}{\partial \theta }\sin \phi \right) \\ F_{89}&=\frac{\partial f_8}{\partial \psi }=\left( \frac{\partial \omega _y}{\partial \psi }\cos \phi -\frac{\partial \omega _z}{\partial \psi }\sin \phi \right) \\ F_{91}&=\frac{\partial f_9}{\partial L}=\left( \frac{\partial \omega _y}{\partial L}\sin \phi +\frac{\partial \omega _z}{\partial L} \cos \phi \right) \sec \theta \\ F_{93}&=\frac{\partial f_9}{\partial Z}=\left( \frac{\partial \omega _y}{\partial Z}\sin \phi +\frac{\partial \omega _z}{\partial Z} \cos \phi \right) \sec \theta \\ F_{94}&=\frac{\partial f_9}{\partial v_N}=\left( \frac{\partial \omega _y}{\partial v_N}\sin \phi +\frac{\partial \omega _z}{\partial v_N} \cos \phi \right) \sec \theta \\ F_{95}&=\frac{\partial f_9}{\partial v_E}=\left( \frac{\partial \omega _y}{\partial v_E}\sin \phi +\frac{\partial \omega _z}{\partial v_E} \cos \phi \right) \sec \theta \\ F_{96}&=\frac{\partial f_9}{\partial v_D}=\left( \frac{\partial \omega _y}{\partial v_D}\sin \phi +\frac{\partial \omega _z}{\partial v_D} \cos \phi \right) \sec \theta \\ F_{97}&=\frac{\partial f_9}{\partial \phi }=\left( \frac{\partial \omega _y}{\partial \phi }\sin \phi +\omega _y \cos \phi \right) \sec \theta \\&\qquad +\left( \frac{\partial \omega _z}{\partial \phi }\cos \phi -\omega _z \sin \phi \right) \sec \theta \\ F_{98}&=\frac{\partial f_9}{\partial \theta }=\left( \frac{\partial \omega _y}{\partial \theta }\sin \phi +\frac{\partial \omega _z}{\partial \theta }\cos \phi \right) \sec \theta \\&\qquad +\left( \omega _y \sin \phi +\omega _z \cos \phi \right) \sec \theta \tan \theta \\ F_{99}&=\frac{\partial f_9}{\partial \psi }=\left( \frac{\partial \omega _y}{\partial \psi }\sin \phi +\frac{\partial \omega _z}{\partial \psi } \cos \phi \right) \sec \theta \end{aligned}$$

where the following derivatives can be calculated

$$\begin{aligned} \frac{\partial R_N}{\partial L}&=\frac{3}{2}e^2\sin (2L)R(1-e^2)(1-e^2\sin ^2L)^{-\frac{5}{2}}\\ \frac{\partial R_E}{\partial L}&=\frac{1}{2}e^2\sin (2L)R(1-e^2\sin ^2L)^{-\frac{3}{2}}\\ \frac{\partial C_{11}}{\partial \phi }&=0 \\ \frac{\partial C_{12}}{\partial \phi }&=\sin \phi \sin \psi + \cos \phi \sin \theta \cos \psi \\ \frac{\partial C_{13}}{\partial \phi }&=\cos \phi \sin \psi - \sin \phi \sin \theta \cos \psi \\ \end{aligned}$$
$$\begin{aligned} \frac{\partial C_{11}}{\partial \theta }&=-\sin \theta \cos \psi \\ \frac{\partial C_{12}}{\partial \theta }&=\sin \phi \cos \theta \cos \psi \\ \frac{\partial C_{13}}{\partial \theta }&=\cos \phi \cos \theta \cos \psi \\ \frac{\partial C_{11}}{\partial \psi }&=-\cos \theta \sin \psi \\ \frac{\partial C_{12}}{\partial \psi }&=-\cos \phi \cos \psi - \sin \phi \sin \theta \sin \psi \\ \frac{\partial C_{13}}{\partial \psi }&=\sin \phi \cos \psi - \cos \phi \sin \theta \sin \psi \\ \frac{\partial C_{21}}{\partial \phi }&=0 \\ \end{aligned}$$
$$\begin{aligned}&\frac{\partial C_{22}}{\partial \phi }=-\sin \phi \cos \psi + \cos \phi \sin \theta \sin \psi \\&\frac{\partial C_{23}}{\partial \phi }=-\cos \phi \cos \psi - \sin \phi \sin \theta \sin \psi \\&\frac{\partial C_{21}}{\partial \theta }=-\sin \theta \sin \psi \\&\frac{\partial C_{22}}{\partial \theta }=\sin \phi \cos \theta \sin \psi \\&\frac{\partial C_{23}}{\partial \theta }=\cos \phi \cos \theta \sin \psi \\&\frac{\partial C_{21}}{\partial \psi }=\cos \theta \cos \psi \\&\frac{\partial C_{22}}{\partial \psi }=-\cos \phi \sin \psi + \sin \phi \sin \theta \cos \psi \\&\frac{\partial C_{23}}{\partial \psi }=\sin \phi \sin \psi + \cos \phi \sin \theta \cos \psi \\&\frac{\partial C_{31}}{\partial \phi }=0\\&\frac{\partial C_{32}}{\partial \phi }=\cos \phi \cos \theta \\&\frac{\partial C_{33}}{\partial \phi }=-\sin \phi \cos \theta \\&\frac{\partial C_{31}}{\partial \theta }=-\cos \theta \\&\frac{\partial C_{32}}{\partial \theta }=-\sin \phi \sin \theta \\&\frac{\partial C_{33}}{\partial \theta }=-\cos \phi \sin \theta \\&\frac{\partial C_{31}}{\partial \psi }=\frac{\partial C_{32}}{\partial \psi }=\frac{\partial C_{33}}{\partial \psi }=0\\&\frac{\partial \omega _x}{\partial L}=-\left( C_{11}\frac{\partial \omega _N}{\partial L}+C_{21}\frac{\partial \omega _E}{\partial L}+C_{31}\frac{\partial \omega _D}{\partial L}\right) \\&\frac{\partial \omega _x}{\partial l}=0\\ \end{aligned}$$
$$\begin{aligned} \frac{\partial \omega _x}{\partial Z}&=-\left( C_{11}\frac{\partial \omega _N}{\partial Z}+C_{21}\frac{\partial \omega _E}{\partial Z}+C_{31}\frac{\partial \omega _D}{\partial Z}\right) \\ \frac{\partial \omega _x}{\partial v_N}&=\left( C_{21}\frac{\partial \omega _E}{\partial v_N}\right) \\ \frac{\partial \omega _x}{\partial v_E}&=-\left( C_{11}\frac{\partial \omega _N}{\partial v_E}+C_{31}\frac{\partial \omega _D}{\partial v_E}\right) \\ \frac{\partial \omega _x}{\partial v_D}&=0\\ \frac{\partial \omega _x}{\partial \phi }&=-\left( C_{11}\frac{\partial \omega _N}{\partial \phi }+C_{21}\frac{\partial \omega _E}{\partial \phi }+C_{31}\frac{\partial \omega _D}{\partial \phi }\right) \\ \frac{\partial \omega _x}{\partial \theta }&=-\left( C_{11}\frac{\partial \omega _N}{\partial \theta }+C_{21}\frac{\partial \omega _E}{\partial \theta }+C_{31}\frac{\partial \omega _D}{\partial \theta }\right) \\ \frac{\partial \omega _x}{\partial \psi }&=-\left( C_{11}\frac{\partial \omega _N}{\partial \psi }+C_{21}\frac{\partial \omega _E}{\partial \psi }+C_{31}\frac{\partial \omega _D}{\partial \psi }\right) \\ \frac{\partial \omega _y}{\partial L}&=-\left( C_{12}\frac{\partial \omega _N}{\partial L}+C_{22}\frac{\partial \omega _E}{\partial L}+C_{32}\frac{\partial \omega _D}{\partial L}\right) \\ \frac{\partial \omega _y}{\partial l}&=0\\ \frac{\partial \omega _y}{\partial Z}&=-\left( C_{12}\frac{\partial \omega _N}{\partial Z}+C_{22}\frac{\partial \omega _E}{\partial Z}+C_{32}\frac{\partial \omega _D}{\partial Z}\right) \\ \frac{\partial \omega _y}{\partial v_N}&=\left( C_{22}\frac{\partial \omega _E}{\partial v_N}\right) \\ \frac{\partial \omega _y}{\partial v_E}&=-\left( C_{12}\frac{\partial \omega _N}{\partial v_E}+C_{32}\frac{\partial \omega _D}{\partial v_E}\right) \\ \frac{\partial \omega _y}{\partial v_D}&=0\\ \frac{\partial \omega _y}{\partial \phi }&=-\left( C_{12}\frac{\partial \omega _N}{\partial \phi }+C_{22}\frac{\partial \omega _E}{\partial \phi }+C_{32}\frac{\partial \omega _D}{\partial \phi }\right) \end{aligned}$$
$$\begin{aligned} \frac{\partial \omega _y}{\partial \theta }&=-\left( C_{12}\frac{\partial \omega _N}{\partial \theta }+C_{22}\frac{\partial \omega _E}{\partial \theta }+C_{32}\frac{\partial \omega _D}{\partial \theta }\right) \\ \frac{\partial \omega _y}{\partial \psi }&=-\left( C_{12}\frac{\partial \omega _N}{\partial \psi }+C_{22}\frac{\partial \omega _E}{\partial \psi }+C_{32}\frac{\partial \omega _D}{\partial \psi }\right) \\ \frac{\partial \omega _x}{\partial L}&=-\left( C_{13}\frac{\partial \omega _N}{\partial L}+C_{23}\frac{\partial \omega _E}{\partial L}+C_{33}\frac{\partial \omega _D}{\partial L}\right) \\ \frac{\partial \omega _x}{\partial l}&=0\\ \frac{\partial \omega _x}{\partial Z}&=-\left( C_{13}\frac{\partial \omega _N}{\partial Z}+C_{23}\frac{\partial \omega _E}{\partial Z}+C_{33}\frac{\partial \omega _D}{\partial Z}\right) \\ \frac{\partial \omega _x}{\partial v_N}&=\left( C_{23}\frac{\partial \omega _E}{\partial v_N}\right) \\ \frac{\partial \omega _x}{\partial v_E}&=-\left( C_{13}\frac{\partial \omega _N}{\partial v_E}+C_{33}\frac{\partial \omega _D}{\partial v_E}\right) \end{aligned}$$
$$\begin{aligned}&\frac{\partial \omega _x}{\partial v_D}=0\\&\frac{\partial \omega _x}{\partial \phi }=-\left( C_{13}\frac{\partial \omega _N}{\partial \phi }+C_{23}\frac{\partial \omega _E}{\partial \phi }+C_{33}\frac{\partial \omega _D}{\partial \phi }\right) \\&\frac{\partial \omega _x}{\partial \theta }=-\left( C_{13}\frac{\partial \omega _N}{\partial \theta }+C_{23}\frac{\partial \omega _E}{\partial \theta }+C_{33}\frac{\partial \omega _D}{\partial \theta }\right) \\&\frac{\partial \omega _x}{\partial \psi }=-\left( C_{13}\frac{\partial \omega _N}{\partial \psi }+C_{23}\frac{\partial \omega _E}{\partial \psi }+C_{33}\frac{\partial \omega _D}{\partial \psi }\right) \\&\frac{\partial \omega _N}{\partial L}=-\varOmega \sin L - \frac{\partial R_E}{\partial L}\frac{v_E}{(R_E+Z)^2}\\&\frac{\partial \omega _N}{\partial Z}=-\frac{v_E}{(R_E+Z)^2}\\&\frac{\partial \omega _E}{\partial L}=- \frac{\partial R_N}{\partial L}\frac{v_N}{(R_ENZ)^2}\\&\frac{\partial \omega _E}{\partial Z}=\frac{v_N}{(R_N+Z)^2}\\&\frac{\partial \omega _E}{\partial v_N}=-\frac{1}{R_N+Z}\\&\frac{\partial \omega _D}{\partial L}=-\varOmega \cos L + \frac{\partial R_E}{\partial L}\frac{v_E \tan L}{(R_E+Z)^2}-\frac{v_E \sec ^2L}{R_E+Z}\\&\frac{\partial \omega _D}{\partial Z}=\frac{v_E \tan L}{(R_E+Z)^2}\\&\frac{\partial \omega _D}{\partial v_E}=\frac{-\tan L}{R_E+Z} \end{aligned}$$

Taking derivatives of \(v_x\), \(v_y\), and \(v_z\) with respect to the orientation states gives

$$\begin{aligned} H_{17}&=\frac{\partial v_x}{\partial \phi }=\frac{\partial C_{11}}{\partial \phi }v_N+\frac{\partial C_{21}}{\partial \phi }v_E+\frac{\partial C_{31}}{\partial \phi }v_D\\ H_{18}&=\frac{\partial v_x}{\partial \theta }=\frac{\partial C_{11}}{\partial \theta }v_N+\frac{\partial C_{21}}{\partial \theta }v_E+\frac{\partial C_{31}}{\partial \theta }v_D\\ H_{19}&=\frac{\partial v_x}{\partial \psi }=\frac{\partial C_{11}}{\partial \psi }v_N+\frac{\partial C_{21}}{\partial \psi }v_E+\frac{\partial C_{31}}{\partial \psi }v_D\\ H_{27}&=\frac{\partial v_y}{\partial \phi }=\frac{\partial C_{12}}{\partial \phi }v_N+\frac{\partial C_{22}}{\partial \phi }v_E+\frac{\partial C_{32}}{\partial \phi }v_D\\ H_{28}&=\frac{\partial v_y}{\partial \theta }=\frac{\partial C_{12}}{\partial \theta }v_N+\frac{\partial C_{22}}{\partial \theta }v_E+\frac{\partial C_{32}}{\partial \theta }v_D\\ H_{29}&=\frac{\partial v_y}{\partial \psi }=\frac{\partial C_{12}}{\partial \psi }v_N+\frac{\partial C_{22}}{\partial \psi }v_E+\frac{\partial C_{32}}{\partial \psi }v_D\\ H_{37}&=\frac{\partial v_z}{\partial \phi }=\frac{\partial C_{13}}{\partial \phi }v_N+\frac{\partial C_{23}}{\partial \phi }v_E+\frac{\partial C_{33}}{\partial \phi }v_D\\ H_{38}&=\frac{\partial v_z}{\partial \theta }=\frac{\partial C_{13}}{\partial \theta }v_N+\frac{\partial C_{23}}{\partial \theta }v_E+\frac{\partial C_{33}}{\partial \theta }v_D\\ H_{39}&=\frac{\partial v_z}{\partial \psi }=\frac{\partial C_{13}}{\partial \psi }v_N+\frac{\partial C_{23}}{\partial \psi }v_E+\frac{\partial C_{33}}{\partial \psi }v_D \end{aligned}$$

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Shabani, M., Gholami, A. & Davari, N. Asynchronous direct Kalman filtering approach for underwater integrated navigation system. Nonlinear Dyn 80, 71–85 (2015). https://doi.org/10.1007/s11071-014-1852-9

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s11071-014-1852-9

Keywords

Search

Navigation