# Five-State Extended Kalman Filter for Estimation of Speed over Ground (SOG), Course over Ground (COG) and Course Rate of Unmanned Surface Vehicles (USVs): Experimental Results

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Kinematics

**Definition**

**1.**

**(Yaw or heading angle $\psi $)**The angle ψ from the ${x}_{n}$ axis (true North) to the ${x}_{b}$ axis of the USV, positive rotation about the ${z}_{n}$ axis by the right-hand screw convention.

**Definition**

**2.**

**(Course angle $\chi \phantom{\rule{0.166667em}{0ex}}$)**The angle χ from the ${x}_{n}$ axis (true North) to the velocity vector of the USV, positive rotation about the ${z}_{n}$ axis by the right-hand screw convention.

**Definition**

**3.**

**(Crab angle ${\beta}_{c}$)**The angle ${\beta}_{c}$ from the ${x}_{b}$ axis to the velocity vector of the USV, positive rotation about the ${z}_{b}$ axis by the right-hand screw convention.

## 3. EKF for SOG, COG and Course Rate

#### 3.1. Five-State EKF: North-East Position Measurements

#### 3.2. Five-State EKF: Latitude and Longitude Measurements

## 4. USV Course Autopilot Design

## 5. Simulation Study of the Otter USV

`xdot = otter(x,n,mp,rp,V_c,beta_c)`

`x = [u,v,w,p,q,r,x,y,z,phi,theta,psi]’`

`n = [n1,n2]’`, the mass of the payload,

`m_p`, the location of the payload,

`r_p = [x_p,y_p,z_p]’`, ocean current speed,

`V_c`, and ocean current direction,

`beta_c`. The toolbox also has a Simulink block for numerical integration of the m-file function.

#### Estimation of SOG, COG and Course Rate during Course Autopilot Control

## 6. Experiments with the Mariner USV

## 7. Experiments with the Otter USV

## 8. Discussion

## 9. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Acknowledgments

## Conflicts of Interest

## Appendix A. Matlab Function: EKF5states.m

## References

- Fossen, T.I. Marine Craft Hydrodynamics and Motion Control; John Wiley & Sons.: Chichchester, UK, 2021. [Google Scholar]
- Farrell, J.A. Aided Navigation: GPS with High Rate Sensors; McGraw-Hill: New York, NY, USA, 2008. [Google Scholar]
- Maritime Robotics AS. 2021. Available online: https://www.maritimerobotics.com (accessed on 25 October 2021).
- Li, X.R.; Jilkov, V. Survey of Maneuvering Target Tracking. Part I. Dynamic Models. IEEE Trans. Aerosp. Electron. Syst.
**2003**, 39, 1333–1364. [Google Scholar] [CrossRef] - Bar-Shalom, Y.; Li, X.R.; Kirubarajan, T. Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software; John Wiley & Sons.: New York, NY, USA, 2001. [Google Scholar] [CrossRef]
- Siegert, G.; Banyś, P.; Martinez, C.S.; Heymann, F. EKF Based Trajectory Tracking and Integrity Monitoring of AIS Data. In Proceedings of the 2016 IEEE/ION Position, Location and Navigation Symposium (PLANS), Savannah, GA, USA, 11–14 April 2016; 2016; pp. 887–897. [Google Scholar] [CrossRef] [Green Version]
- Fossen, S.; Fossen, T.I. eXogenous Kalman filter (XKF) for Visualization and Motion Prediction of Ships using Live Automatic Identification System (AIS) Data. Model. Identif. Control (MIC)
**2018**, 39, 233–244. [Google Scholar] [CrossRef] [Green Version] - Gade, K. The Seven Ways to Find Heading. R. Inst. Navig.
**2016**, 69, 955–970. [Google Scholar] [CrossRef] [Green Version] - Singer, R.A. Estimating Optimal Tracking Filter Performance for Manned Maneuvering Targets. Trans. Aerosp. Electron. Syst.
**1970**, 6, 473–483. [Google Scholar] [CrossRef] - Brown, R.G.; Hwang, Y.C. Introduction to Random Signals and Applied Kalman Filtering; John Wiley & Sons.: New York, NY, USA, 2012. [Google Scholar]
- Department of Defense. World Geodetic System 1984—Its Definition and Relationships with Local Geodetic Systems, DMA TR 8350.2, 2nd ed.; National Imagery and Mapping Agency (NIMA): Springfield, VA, USA, 23 June 2004. [Google Scholar]
- Nomoto, K.; Taguchi, T.; Honda, K.; Hirano, S. On the Steering Qualities of Ships. Tech. Report. Int. Shipbuild. Prog.
**1957**, 4, 354–370. [Google Scholar] [CrossRef] - Fossen, T.I.; Perez, T. Marine Systems Simulator (MSS). 2014. Available online: https://github.com/cybergalactic/MSS (accessed on 25 October 2021).
- Bhat, S.; Bernstein, D.S. A Topological Obstruction to Continuous Global Stabilization of Rotational Motion and the Unwinding Phenomenon. Syst. Control Lett.
**2000**, 39, 63–70. [Google Scholar] [CrossRef] - u-Blox. 2021. Available online: https://www.u-blox.com (accessed on 25 October 2021).
- Matlab. Mathworks. Available online: https://www.mathworks.com (accessed on 25 October 2021).

**Figure 1.**The Maritime Robotics Otter Pro USV (dimensions 2.00 × 1.07 × 0.82 m and weight 65 kg) [3].

**Figure 2.**The Maritime Robotics Mariner USV (dimensions 5.95 × 2.05 × 2.00 m and weight 1900 kg) [3].

**Figure 4.**Simulation study: North–East positions of the USV for a 20 degrees course command, which is changed back to 0 degrees at time $t=20$ s.

**Figure 5.**Simulation study: Propeller revolutions ${n}_{1}$ and ${n}_{2}$ versus time during autopilot control.

**Figure 6.**Simulation study: Estimated SOG, COG, and course rate versus time. The zoomed plots to the right show the slow GNSS rate (5 Hz) compared to the EKF sampling time (50 Hz). The autopilot performed a 20 degrees course command, which was returned to 0 degrees at time $t=20$ s.

**Figure 7.**The blue line shows the path of the Mariner USV. The starting point is Vanvikan located north of Trondheim in the Trondheim fjord at ${63}^{\circ}\phantom{\rule{0.222222em}{0ex}}{33}^{\prime}\phantom{\rule{0.222222em}{0ex}}{11.23}^{\prime}\phantom{\rule{3.33333pt}{0ex}}\mathrm{N}$ and ${10}^{\circ}\phantom{\rule{0.222222em}{0ex}}{14}^{\prime}\phantom{\rule{0.222222em}{0ex}}{2.90}^{\prime \prime}\phantom{\rule{3.33333pt}{0ex}}\mathrm{E}$.

**Figure 8.**Experiment with the Mariner USV. Estimated SOG and COG versus time using latitude and longitude measurements at 5 Hz. Both the GNSS receiver and the EKF estimate the SOG and COG quite well for speeds higher than 1 m/s. The only discrepancy is during start up (low speed).

**Figure 9.**The blue line shows the path of the Otter USV when operating in the harbor (upper plot) and in the middle of the Trondheim fjord (lower plot). The starting points are ${63}^{\circ}\phantom{\rule{0.222222em}{0ex}}{26}^{\prime}\phantom{\rule{0.222222em}{0ex}}{22.37}^{\prime \prime}\phantom{\rule{3.33333pt}{0ex}}\mathrm{N}$ and ${10}^{\circ}\phantom{\rule{0.222222em}{0ex}}{24}^{\prime}\phantom{\rule{0.222222em}{0ex}}{1.49}^{\prime \prime}\phantom{\rule{3.33333pt}{0ex}}\mathrm{E}$ and ${63}^{\circ}\phantom{\rule{0.222222em}{0ex}}{27}^{\prime}\phantom{\rule{0.222222em}{0ex}}{0.04}^{\prime \prime}\phantom{\rule{3.33333pt}{0ex}}\mathrm{N}$ and ${10}^{\circ}\phantom{\rule{0.222222em}{0ex}}{24}^{\prime}\phantom{\rule{0.222222em}{0ex}}{0.17}^{\prime \prime}\phantom{\rule{3.33333pt}{0ex}}\mathrm{E}$, respectively.

**Figure 10.**Experiment with the Otter USV in the middle of the Trondheim fjord. Estimated SOG and COG versus time using latitude and longitude measurements at 5 Hz.

**Figure 11.**Experiment with the Otter USV in the harbor. Estimated SOG and COG versus time using latitude and longitude measurements at 5 Hz. The results are good when the vehicle moves at approximately 1.0 m/s and less accurate at very low speeds.

Initial values | $\phantom{\rule{1.0pt}{0ex}}{\widehat{\mathit{x}}}^{-}\left[0\right]={\mathit{x}}_{0}$ |
---|---|

${\widehat{\mathit{P}}}^{-}\left[0\right]=\mathrm{E}\left[(\mathit{x}\left[0\right]-{\widehat{\mathit{x}}}^{-}\left[0\right]){(\mathit{x}\left[0\right]-{\widehat{\mathit{x}}}^{-}\left[0\right])}^{\top}\right]={\mathit{P}}_{0}$ | |

Kalman filter gain matrix | $\mathit{K}\left[k\right]={\widehat{\mathit{P}}}^{-}\left[k\right]{\mathit{C}}_{d}^{\top}\left[k\right]{\left({\mathit{C}}_{d}\left[k\right]{\widehat{\mathit{P}}}^{-}\left[k\right]{\mathit{C}}_{d}^{\top}\left[k\right]+{\mathit{R}}_{d}\left[k\right]\right)}^{-1}$ |

State vector corrector | $\widehat{\mathit{x}}\left[k\right]={\widehat{\mathit{x}}}^{-}\left[k\right]+\mathit{K}\left[k\right]\left(\mathit{y}\left[k\right]-\mathit{h}\left({\widehat{\mathit{x}}}^{-}\left[k\right]\right)\right)$ |

Covariance matrix corrector | $\widehat{\mathit{P}}\left[k\right]=\left(\mathit{I}-\mathit{K}\left[k\right]{\mathit{C}}_{d}\left[k\right]\right){\widehat{\mathit{P}}}^{-}\left[k\right]{\left(\mathit{I}-\mathit{K}\left[k\right]{\mathit{C}}_{d}\left[k\right]\right)}^{\phantom{\rule{-0.166667em}{0ex}}\phantom{\rule{-0.166667em}{0ex}}\top}+\mathit{K}\left[k\right]{\mathit{R}}_{d}\left[k\right]{\mathit{K}}^{\phantom{\rule{-0.166667em}{0ex}}\top}\phantom{\rule{-0.166667em}{0ex}}\left[k\right]$ |

State vector predictor | ${\widehat{\mathit{x}}}^{-}[k+1]={\mathit{A}}_{d}\phantom{\rule{0.222222em}{0ex}}\widehat{\mathit{x}}\left[k\right]+{\mathit{B}}_{d}\mathit{u}\left[k\right]$ |

Covariance matrix predictor | ${\widehat{\mathit{P}}}^{-}[k+1]={\mathit{A}}_{d}\widehat{\mathit{P}}\left[k\right]{\mathit{A}}_{d}^{\top}+{\mathit{E}}_{d}{\mathit{Q}}_{d}\left[k\right]{\mathit{E}}_{d}^{\top}$ |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Fossen, S.; Fossen, T.I.
Five-State Extended Kalman Filter for Estimation of Speed over Ground (SOG), Course over Ground (COG) and Course Rate of Unmanned Surface Vehicles (USVs): Experimental Results. *Sensors* **2021**, *21*, 7910.
https://doi.org/10.3390/s21237910

**AMA Style**

Fossen S, Fossen TI.
Five-State Extended Kalman Filter for Estimation of Speed over Ground (SOG), Course over Ground (COG) and Course Rate of Unmanned Surface Vehicles (USVs): Experimental Results. *Sensors*. 2021; 21(23):7910.
https://doi.org/10.3390/s21237910

**Chicago/Turabian Style**

Fossen, Sindre, and Thor I. Fossen.
2021. "Five-State Extended Kalman Filter for Estimation of Speed over Ground (SOG), Course over Ground (COG) and Course Rate of Unmanned Surface Vehicles (USVs): Experimental Results" *Sensors* 21, no. 23: 7910.
https://doi.org/10.3390/s21237910