Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (2024)

Artur Wolekโ€ƒโ€ƒJames McMahonDepartment of Mechanical Engineering and Engineering Science, University of North Carolina at Charlotte, Charlotte, NC, 28223 USA (e-mail: awolek@charlotte.edu).Physical Acoustics Branch, Code 7135, Naval Research Laboratory, Washington, DC, 20375 USA (e-mail: james.w.mcmahon8.civ@us.navy.mil)

Abstract

This paper presents three batch estimation methods that use noisy ground velocity and heading measurements from a vehicle executing a circular orbit (or similar large heading change maneuver) to estimate the speed and direction of a steady, uniform, flow-field. The methods are based on a simple kinematic model of the vehicleโ€™s motion and use curve-fitting or nonlinear least-square optimization. A Monte Carlo simulation with randomized flow conditions is used to evaluate the batch estimation methods while varying the measurement noise of the data and the interval of unique heading traversed during the maneuver. The methods are also compared using experimental data obtained with a Bluefin-21 unmanned underwater vehicle performing a series of circular orbit maneuvers over a five hour period in a tide-driven flow.

keywords:

marine vehicle, velocity triangle, current estimation, least-square regression

ยฉ 2024 Artur Wolek and James McMahon. This work has been accepted to IFAC for publication under a Creative Commons Licence CC-BY-NC-ND

โ€ โ€ thanks: This work was supported in part by NSF Grant No. 2301475.

1 Introduction

Autonomy and control algorithms can leverage estimates of the flow-field to improve the operation of marine vehicles, for example, in path planning to achieve greater efficiency and robustness. This paper is motivated by the authorsโ€™ prior work (Wolek etal., 2021) that investigated time-optimal path planning for an underwater vehicle to re-inspect points of interest along circular orbits with a sonar sensor. In such applications, the presence of a flow-field impacts the vehicle motion and the pointing angle of a directional sonar sensor (due to sideslip). The methods presented here can be used to estimate flow direction and magnitude on-the-fly (e.g., after executing each circular inspection orbit) to support path re-planning.

For a vehicle operating in a steady, uniform, flow-field the flow-relative velocity ๐’—relsubscript๐’—rel{\bm{v}}_{\rm rel}bold_italic_v start_POSTSUBSCRIPT roman_rel end_POSTSUBSCRIPT, the flow velocity ๐’˜๐’˜{\bm{w}}bold_italic_w, and the inertial (ground) velocity ๐’—gsubscript๐’—g{\bm{v}}_{\rm g}bold_italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT are related by the equation ๐’—g=๐’—rel+๐’˜subscript๐’—gsubscript๐’—rel๐’˜{\bm{v}}_{\rm g}={\bm{v}}_{\rm rel}+{\bm{w}}bold_italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT = bold_italic_v start_POSTSUBSCRIPT roman_rel end_POSTSUBSCRIPT + bold_italic_w, as illustrated in Fig.1. When two of the three quantities are known, the remaining one can be determined by direct computation. For marine vehicles, ground velocity, ๐’—gsubscript๐’—g{\bm{v}}_{\rm g}bold_italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT, is provided by the navigation system by processing GPS position measurements, acoustic ranging measurements and/or a Doppler velocity log (DVL) sensor operating in bottom-lock mode. The vehicle flow-relative velocity ๐’—relsubscript๐’—rel{\bm{v}}_{\rm rel}bold_italic_v start_POSTSUBSCRIPT roman_rel end_POSTSUBSCRIPT can also be measured with appropriate instrumentation (e.g., pitot tubes or a DVL operating in water current profiling mode).The flow velocity ๐’˜๐’˜{\bm{w}}bold_italic_w may be inferred using various filtering techniques based on the vehicleโ€™s kinematics (Rhudy etal., 2015) or a dynamic model (Hegrenaes and Hallingstad, 2011). Non-uniform flow-fields can be estimated, for example, using motion tomography (Chang etal., 2017).

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (1)

The contributions of this paper are three batch estimation techniques to estimate water current direction and magnitude for a marine craft using noisy measurements of kinematic variables and a basic model of the vehicleโ€™s motion. The methods do not require knowledge of the vehicleโ€™s flow-relative speed (since it is estimated as part of the procedure) and they are relatively simple to implement with minimal tuning required. The first method is based on quadratic curve fitting to speed-over-ground magnitude and heading angle data when the vehicle executes a 360 degree heading change maneuver (e.g., a circular orbit). The second method relies on a constrained nonlinear least-square optimization to match speed-over-ground velocity components and heading data. The third method is similar to the second but uses speed-over-ground magnitude, rather than velocity components, for optimization. The approaches are compared to each other, as well as to another existing method in the literature based on circular curve fitting, using both simulated and experimental data.

The remainder of the paper is organized as follows. Section2 describes two curve-fitting based methods. Section3 describes two optimization-based methods. Sections4 and 5 provide a comparison with simulated and experimental data, respectively. Section6 concludes the paper.

2 Curve-Fitting-Based Methods

Consider the following kinematic equations describing the motion of a marine craft in the horizontal plane:

xห™โข(t)ห™๐‘ฅ๐‘ก\displaystyle\dot{x}(t)overห™ start_ARG italic_x end_ARG ( italic_t )=fxโข(ฯˆ;v,w,ฮธ)=vโขcosโกฯˆโข(t)+wโขcosโกฮธabsentsubscript๐‘“๐‘ฅ๐œ“๐‘ฃ๐‘ค๐œƒ๐‘ฃ๐œ“๐‘ก๐‘ค๐œƒ\displaystyle=f_{x}(\psi;v,w,\theta)=v\cos\psi(t)+w\cos\theta= italic_f start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT ( italic_ฯˆ ; italic_v , italic_w , italic_ฮธ ) = italic_v roman_cos italic_ฯˆ ( italic_t ) + italic_w roman_cos italic_ฮธ(1)
yห™โข(t)ห™๐‘ฆ๐‘ก\displaystyle\dot{y}(t)overห™ start_ARG italic_y end_ARG ( italic_t )=fyโข(ฯˆ;v,w,ฮธ)=vโขsinโกฯˆโข(t)+wโขsinโกฮธ,absentsubscript๐‘“๐‘ฆ๐œ“๐‘ฃ๐‘ค๐œƒ๐‘ฃ๐œ“๐‘ก๐‘ค๐œƒ\displaystyle=f_{y}(\psi;v,w,\theta)=v\sin\psi(t)+w\sin\theta\;,= italic_f start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT ( italic_ฯˆ ; italic_v , italic_w , italic_ฮธ ) = italic_v roman_sin italic_ฯˆ ( italic_t ) + italic_w roman_sin italic_ฮธ ,(2)

where (x,y)๐‘ฅ๐‘ฆ(x,y)( italic_x , italic_y ) is the planar position, v=โ€–๐’—relโ€–๐‘ฃnormsubscript๐’—relv=||{\bm{v}}_{\rm rel}||italic_v = | | bold_italic_v start_POSTSUBSCRIPT roman_rel end_POSTSUBSCRIPT | | is the flow-relative speed, w=โ€–๐’˜โ€–๐‘คnorm๐’˜w=||{\bm{w}}||italic_w = | | bold_italic_w | | is the magnitude of the current, and ฮธ๐œƒ\thetaitalic_ฮธ is the current direction. The speed v๐‘ฃvitalic_v is assumed constant (e.g., corresponding to a fixed propeller speed). The current magnitude and direction, (w,ฮธ)๐‘ค๐œƒ(w,\theta)( italic_w , italic_ฮธ ), is assumed to be steady and uniform over the space in which a heading change maneuver is performed.This section describes two methods for estimating (v,w,ฮธ)๐‘ฃ๐‘ค๐œƒ(v,w,\theta)( italic_v , italic_w , italic_ฮธ ) in (1)โ€“(2) based on curve fitting to noisy data of either ground velocity components or ground velocity magnitude as a function of heading.

2.1 Circular Curve Fit with (xห™,yห™)ห™๐‘ฅห™๐‘ฆ(\dot{x},\dot{y})( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG ) Data

In (McLaren, 2008) it was shown that given a minimum of three unique ground velocity component measurements, ๐’—=[xห™,yห™]T๐’—superscriptห™๐‘ฅห™๐‘ฆT{\bm{v}}=[\dot{x},\dot{y}]^{\rm T}bold_italic_v = [ overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG ] start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT from (1)โ€“(2), the parameters (v,w,ฮธ)๐‘ฃ๐‘ค๐œƒ(v,w,\theta)( italic_v , italic_w , italic_ฮธ ) can be estimated. Here, we summarize this approach and use it for comparisons later on in Secs.4 and 5.

Consider the sketch in Fig.1 (right panel) showing the velocity triangle and a triplet of velocity triangles for three different flow-relative velocities. A circle that passes through the three ground velocity vectors can be inscribed and its center corresponds to the current velocity ๐’˜=[wx,wy]T=[wโขcosโกฮธ,wโขsinโกฮธ]T๐’˜superscriptsubscript๐‘ค๐‘ฅsubscript๐‘ค๐‘ฆTsuperscript๐‘ค๐œƒ๐‘ค๐œƒT{\bm{w}}=[w_{x},w_{y}]^{\rm T}=[w\cos\theta,w\sin\theta]^{\rm T}bold_italic_w = [ italic_w start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT , italic_w start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT ] start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT = [ italic_w roman_cos italic_ฮธ , italic_w roman_sin italic_ฮธ ] start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT. The radius of the circle is the vehicleโ€™s flow-relative speed v๐‘ฃvitalic_v.One can confirm, by substituting (1)โ€“(2), that all points on the circle satisfy

(xห™โˆ’wx)2+(yห™โˆ’wy)2superscriptห™๐‘ฅsubscript๐‘ค๐‘ฅ2superscriptห™๐‘ฆsubscript๐‘ค๐‘ฆ2\displaystyle(\dot{x}-w_{x})^{2}+(\dot{y}-w_{y})^{2}( overห™ start_ARG italic_x end_ARG - italic_w start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + ( overห™ start_ARG italic_y end_ARG - italic_w start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT=v2,absentsuperscript๐‘ฃ2\displaystyle=v^{2}\;,= italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ,(3)

which can re-arranged as

โˆ’2โขwxโขxห™โˆ’2โขwyโขyห™+(โˆ’v2+wx2+wy2)2subscript๐‘ค๐‘ฅห™๐‘ฅ2subscript๐‘ค๐‘ฆห™๐‘ฆsuperscript๐‘ฃ2superscriptsubscript๐‘ค๐‘ฅ2superscriptsubscript๐‘ค๐‘ฆ2\displaystyle-2w_{x}\dot{x}-2w_{y}\dot{y}+(-v^{2}+w_{x}^{2}+w_{y}^{2})- 2 italic_w start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT overห™ start_ARG italic_x end_ARG - 2 italic_w start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT overห™ start_ARG italic_y end_ARG + ( - italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT )=โˆ’xห™2โˆ’yห™2.absentsuperscriptห™๐‘ฅ2superscriptห™๐‘ฆ2\displaystyle=-\dot{x}^{2}-\dot{y}^{2}\;.= - overห™ start_ARG italic_x end_ARG start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - overห™ start_ARG italic_y end_ARG start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT .(4)

Suppose that N๐‘Nitalic_N measurements {xห™i,yห™i}i=1Nsuperscriptsubscriptsubscriptห™๐‘ฅ๐‘–subscriptห™๐‘ฆ๐‘–๐‘–1๐‘\{\dot{x}_{i},\dot{y}_{i}\}_{i=1}^{N}{ overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT } start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT are recorded while the vehicle performs a maneuver.In the absence of noise, the data satisfies ๐‘จโข๐’„=๐’ƒ๐‘จ๐’„๐’ƒ{\bm{A}}{\bm{c}}={\bm{b}}bold_italic_A bold_italic_c = bold_italic_b, where

๐‘จ=[xห™iyห™i1โ‹ฎโ‹ฎโ‹ฎxห™Nyห™N1],๐’ƒ=[โˆ’xห™i2โˆ’yห™i2โ‹ฎโˆ’xห™N2โˆ’yห™N2],formulae-sequence๐‘จdelimited-[]subscriptห™๐‘ฅ๐‘–subscriptห™๐‘ฆ๐‘–1โ‹ฎโ‹ฎโ‹ฎsubscriptห™๐‘ฅ๐‘subscriptห™๐‘ฆ๐‘1๐’ƒdelimited-[]superscriptsubscriptห™๐‘ฅ๐‘–2superscriptsubscriptห™๐‘ฆ๐‘–2โ‹ฎsuperscriptsubscriptห™๐‘ฅ๐‘2superscriptsubscriptห™๐‘ฆ๐‘2\displaystyle{\bm{A}}=\left[\begin{array}[]{ccc}\dot{x}_{i}&\dot{y}_{i}&1\\\vdots&\vdots&\vdots\\\dot{x}_{N}&\dot{y}_{N}&1\\\end{array}\right]\;,\qquad{\bm{b}}=\left[\begin{array}[]{c}-\dot{x}_{i}^{2}-%\dot{y}_{i}^{2}\\\vdots\\-\dot{x}_{N}^{2}-\dot{y}_{N}^{2}\\\end{array}\right]\;,bold_italic_A = [ start_ARRAY start_ROW start_CELL overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_CELL start_CELL overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_CELL start_CELL 1 end_CELL end_ROW start_ROW start_CELL โ‹ฎ end_CELL start_CELL โ‹ฎ end_CELL start_CELL โ‹ฎ end_CELL end_ROW start_ROW start_CELL overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_N end_POSTSUBSCRIPT end_CELL start_CELL overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_N end_POSTSUBSCRIPT end_CELL start_CELL 1 end_CELL end_ROW end_ARRAY ] , bold_italic_b = [ start_ARRAY start_ROW start_CELL - overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_CELL end_ROW start_ROW start_CELL โ‹ฎ end_CELL end_ROW start_ROW start_CELL - overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_N end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_N end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_CELL end_ROW end_ARRAY ] ,(11)

and ๐’„=[โˆ’2โขwx,โˆ’2โขwy,โˆ’v2+wx2+wy2]T=[c1,c2,c3]T๐’„superscript2subscript๐‘ค๐‘ฅ2subscript๐‘ค๐‘ฆsuperscript๐‘ฃ2superscriptsubscript๐‘ค๐‘ฅ2superscriptsubscript๐‘ค๐‘ฆ2Tsuperscriptsubscript๐‘1subscript๐‘2subscript๐‘3T{\bm{c}}=[-2w_{x},-2w_{y},-v^{2}+w_{x}^{2}+w_{y}^{2}]^{\rm T}=[c_{1},c_{2},c_{%3}]^{\rm T}bold_italic_c = [ - 2 italic_w start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT , - 2 italic_w start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT , - italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ] start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT = [ italic_c start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT , italic_c start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT , italic_c start_POSTSUBSCRIPT 3 end_POSTSUBSCRIPT ] start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT. If, instead, the data {xห™i,yห™i}i=1Nsuperscriptsubscriptsubscriptห™๐‘ฅ๐‘–subscriptห™๐‘ฆ๐‘–๐‘–1๐‘\{\dot{x}_{i},\dot{y}_{i}\}_{i=1}^{N}{ overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT } start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT is corrupted by zero-mean additive Gaussian random variables with variance ฯƒ2superscript๐œŽ2\sigma^{2}italic_ฯƒ start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT, then the system of equations can only be satisfied in a least-square sense (i.e., minimizing J=โ€–๐‘จโข๐’„โˆ’๐’ƒโ€–2๐ฝsuperscriptnorm๐‘จ๐’„๐’ƒ2J=||{\bm{A}}{\bm{c}}-{\bm{b}}||^{2}italic_J = | | bold_italic_A bold_italic_c - bold_italic_b | | start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT) where ๐’„๐’„{\bm{c}}bold_italic_c is estimated as ๐’„=๐‘จโ€ โข๐’ƒ๐’„superscript๐‘จโ€ ๐’ƒ{\bm{c}}={\bm{A}}^{\rm\dagger}{\bm{b}}bold_italic_c = bold_italic_A start_POSTSUPERSCRIPT โ€  end_POSTSUPERSCRIPT bold_italic_b and ๐‘จโ€ =(๐‘จTโข๐‘จ)โˆ’1superscript๐‘จโ€ superscriptsuperscript๐‘จT๐‘จ1{\bm{A}}^{\dagger}=({\bm{A}}^{\rm T}{\bm{A}})^{-1}bold_italic_A start_POSTSUPERSCRIPT โ€  end_POSTSUPERSCRIPT = ( bold_italic_A start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT bold_italic_A ) start_POSTSUPERSCRIPT - 1 end_POSTSUPERSCRIPT is the matrix pseudo-inverse. The flow-field parameters are then:

w^^๐‘ค\displaystyle\hat{w}over^ start_ARG italic_w end_ARG=12โขc12+c22absent12superscriptsubscript๐‘12superscriptsubscript๐‘22\displaystyle=\frac{1}{2}\sqrt{c_{1}^{2}+c_{2}^{2}}= divide start_ARG 1 end_ARG start_ARG 2 end_ARG square-root start_ARG italic_c start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_c start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_ARG(12)
ฮธ^^๐œƒ\displaystyle\hat{\theta}over^ start_ARG italic_ฮธ end_ARG=atan2โข(c2,c1)absentatan2subscript๐‘2subscript๐‘1\displaystyle={\rm atan2}(c_{2},c_{1})= atan2 ( italic_c start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT , italic_c start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT )(13)
v^^๐‘ฃ\displaystyle\hat{v}over^ start_ARG italic_v end_ARG=(1/4)โข(c12+c22)โˆ’c3.absent14superscriptsubscript๐‘12superscriptsubscript๐‘22subscript๐‘3\displaystyle=\sqrt{(1/4)(c_{1}^{2}+c_{2}^{2})-c_{3}}\;.= square-root start_ARG ( 1 / 4 ) ( italic_c start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_c start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ) - italic_c start_POSTSUBSCRIPT 3 end_POSTSUBSCRIPT end_ARG .(14)

An illustrative example of fitting a circular curve to (xห™,yห™)ห™๐‘ฅห™๐‘ฆ(\dot{x},\dot{y})( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG ) data using this method is shown in Fig.2 for a maneuver where the heading completes one full revolution (i.e., ฮ”โขฯˆ=2โขฯ€ฮ”๐œ“2๐œ‹\Delta\psi=2\piroman_ฮ” italic_ฯˆ = 2 italic_ฯ€). The approach can also be applied for maneuvers with ฮ”โขฯˆ<2โขฯ€ฮ”๐œ“2๐œ‹\Delta\psi<2\piroman_ฮ” italic_ฯˆ < 2 italic_ฯ€.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (2)

2.2 Quadratic Curve Fit with (vg,ฯˆ)subscript๐‘ฃg๐œ“(v_{\rm g},\psi)( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT , italic_ฯˆ ) Data

Now, suppose that the vehicle has access to the ground speed magnitude vg:=โ€–๐’—gโ€–assignsubscript๐‘ฃgnormsubscript๐’—gv_{\rm g}:=||{\bm{v}}_{\rm g}||italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT := | | bold_italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT | | (rather than the velocity components) along with heading angle information. That is, the data is {vg,i,ฯˆi}i=1Nsuperscriptsubscriptsubscript๐‘ฃg๐‘–subscript๐œ“๐‘–๐‘–1๐‘\{v_{{\rm g},i},\psi_{i}\}_{i=1}^{N}{ italic_v start_POSTSUBSCRIPT roman_g , italic_i end_POSTSUBSCRIPT , italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT } start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT where

vg=fvgโข(ฯˆ;v,w,ฮธ)subscript๐‘ฃgsubscript๐‘“subscript๐‘ฃg๐œ“๐‘ฃ๐‘ค๐œƒ\displaystyle v_{\rm g}=f_{v_{\rm g}}(\psi;v,w,\theta)italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT = italic_f start_POSTSUBSCRIPT italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT end_POSTSUBSCRIPT ( italic_ฯˆ ; italic_v , italic_w , italic_ฮธ )=xห™โข(t)2+yห™โข(t)2absentห™๐‘ฅsuperscript๐‘ก2ห™๐‘ฆsuperscript๐‘ก2\displaystyle=\sqrt{\dot{x}(t)^{2}+\dot{y}(t)^{2}}= square-root start_ARG overห™ start_ARG italic_x end_ARG ( italic_t ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + overห™ start_ARG italic_y end_ARG ( italic_t ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_ARG(15)
=v2+w2+2โขvโขwโขcosโก(ฮธโˆ’ฯˆ).absentsuperscript๐‘ฃ2superscript๐‘ค22๐‘ฃ๐‘ค๐œƒ๐œ“\displaystyle=\sqrt{v^{2}+w^{2}+2vw\cos(\theta-\psi)}\;.= square-root start_ARG italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + 2 italic_v italic_w roman_cos ( italic_ฮธ - italic_ฯˆ ) end_ARG .(16)

Again, the data is assumed to be corrupted by additive zero-mean Gaussian noise. In general this assumption is made throughout this work, however, it is worth mentioning that for a vessel continuously rotating in one direction that is not aft-fore symmetric there may be a bias present based on the direction of rotation. To mitigate this bias, the dataset could include maneuvers performed in both clockwise and counter-clockwise orientations.If the heading undergoes a full revolution, then the curve vgโข(ฯˆ)subscript๐‘ฃg๐œ“v_{\rm g}(\psi)italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ( italic_ฯˆ ) contains a maximum and a minimum (see Fig.3). These minima and maxima can be approximated with a quadratic curve fit to estimate (w^,v^,ฮธ^)^๐‘ค^๐‘ฃ^๐œƒ(\hat{w},\hat{v},\hat{\theta})( over^ start_ARG italic_w end_ARG , over^ start_ARG italic_v end_ARG , over^ start_ARG italic_ฮธ end_ARG ) as described next.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (3)

Differentiating (16) with respect to ฯˆ๐œ“\psiitalic_ฯˆ gives the condition for a critical point:

ddโขฯˆโขvgdd๐œ“subscript๐‘ฃg\displaystyle\frac{{\rm d}}{{\rm d}\psi}v_{\rm g}divide start_ARG roman_d end_ARG start_ARG roman_d italic_ฯˆ end_ARG italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT=vโขwโขsinโก(ฮธโˆ’ฯˆ)v2+w2+2โขvโขwโขcosโก(ฮธโˆ’ฯˆ)=0,absent๐‘ฃ๐‘ค๐œƒ๐œ“superscript๐‘ฃ2superscript๐‘ค22๐‘ฃ๐‘ค๐œƒ๐œ“0\displaystyle=\frac{vw\sin(\theta-\psi)}{\sqrt{v^{2}+w^{2}+2vw\cos(\theta-\psi%)}}=0\;,= divide start_ARG italic_v italic_w roman_sin ( italic_ฮธ - italic_ฯˆ ) end_ARG start_ARG square-root start_ARG italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + 2 italic_v italic_w roman_cos ( italic_ฮธ - italic_ฯˆ ) end_ARG end_ARG = 0 ,(17)

which is satisfied at ฯˆ=ฮธ๐œ“๐œƒ\psi=\thetaitalic_ฯˆ = italic_ฮธ and ฯˆ=ฮธ+ฯ€๐œ“๐œƒ๐œ‹\psi=\theta+\piitalic_ฯˆ = italic_ฮธ + italic_ฯ€. The speed over ground is a maximum when the vehicle heading is aligned with the current, vgโข(ฯˆ=ฮธ)=v+wsubscript๐‘ฃg๐œ“๐œƒ๐‘ฃ๐‘คv_{\rm g}(\psi=\theta)=v+witalic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ( italic_ฯˆ = italic_ฮธ ) = italic_v + italic_w, and a minimum when they are opposed, vgโข(ฯˆ=ฮธ+ฯ€)=vโˆ’wsubscript๐‘ฃg๐œ“๐œƒ๐œ‹๐‘ฃ๐‘คv_{\rm g}(\psi=\theta+\pi)=v-witalic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ( italic_ฯˆ = italic_ฮธ + italic_ฯ€ ) = italic_v - italic_w.Now consider a Taylor series approximation at these two points. At the maximum:

vgsubscript๐‘ฃg\displaystyle v_{\rm g}italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPTโ‰ˆ(v+w)โˆ’vโขwv2+2โขvโขw+w2โข(ฯˆโˆ’ฮธ)2absent๐‘ฃ๐‘ค๐‘ฃ๐‘คsuperscript๐‘ฃ22๐‘ฃ๐‘คsuperscript๐‘ค2superscript๐œ“๐œƒ2\displaystyle\approx(v+w)-\frac{vw}{\sqrt{v^{2}+2vw+w^{2}}}(\psi-\theta)^{2}โ‰ˆ ( italic_v + italic_w ) - divide start_ARG italic_v italic_w end_ARG start_ARG square-root start_ARG italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + 2 italic_v italic_w + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_ARG end_ARG ( italic_ฯˆ - italic_ฮธ ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT(18)
=(โˆ’vโขwโขฮด+)โŸa+โขฯˆ2+(2โขฮธโขvโขwโขฮด+)โŸb+โขฯˆ+(v+wโˆ’vโขwโขฮด+โขฮธ2)โŸc+,absentsubscriptโŸ๐‘ฃ๐‘คsuperscript๐›ฟsuperscript๐‘Žsuperscript๐œ“2subscriptโŸ2๐œƒ๐‘ฃ๐‘คsuperscript๐›ฟsuperscript๐‘๐œ“subscriptโŸ๐‘ฃ๐‘ค๐‘ฃ๐‘คsuperscript๐›ฟsuperscript๐œƒ2superscript๐‘\displaystyle=\underbrace{(-vw\delta^{+})}_{a^{+}}\psi^{2}+\underbrace{(2%\theta vw\delta^{+})}_{b^{+}}\psi+\underbrace{(v+w-vw\delta^{+}\theta^{2})}_{c%^{+}}\;,= underโŸ start_ARG ( - italic_v italic_w italic_ฮด start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT ) end_ARG start_POSTSUBSCRIPT italic_a start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT end_POSTSUBSCRIPT italic_ฯˆ start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + underโŸ start_ARG ( 2 italic_ฮธ italic_v italic_w italic_ฮด start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT ) end_ARG start_POSTSUBSCRIPT italic_b start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT end_POSTSUBSCRIPT italic_ฯˆ + underโŸ start_ARG ( italic_v + italic_w - italic_v italic_w italic_ฮด start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT italic_ฮธ start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ) end_ARG start_POSTSUBSCRIPT italic_c start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT end_POSTSUBSCRIPT ,(19)

where ฮด+=1/v2+2โขvโขw+w2superscript๐›ฟ1superscript๐‘ฃ22๐‘ฃ๐‘คsuperscript๐‘ค2\delta^{+}=1/\sqrt{v^{2}+2vw+w^{2}}italic_ฮด start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT = 1 / square-root start_ARG italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + 2 italic_v italic_w + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_ARG. Similarly, at the minimum

vgsubscript๐‘ฃg\displaystyle v_{\rm g}italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPTโ‰ˆ(vโˆ’w)+vโขwv2โˆ’2โขvโขw+w2โข(ฯˆโˆ’(ฮธ+ฯ€))2absent๐‘ฃ๐‘ค๐‘ฃ๐‘คsuperscript๐‘ฃ22๐‘ฃ๐‘คsuperscript๐‘ค2superscript๐œ“๐œƒ๐œ‹2\displaystyle\approx(v-w)+\frac{vw}{\sqrt{v^{2}-2vw+w^{2}}}(\psi-(\theta+\pi))%^{2}โ‰ˆ ( italic_v - italic_w ) + divide start_ARG italic_v italic_w end_ARG start_ARG square-root start_ARG italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - 2 italic_v italic_w + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_ARG end_ARG ( italic_ฯˆ - ( italic_ฮธ + italic_ฯ€ ) ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT(20)
=(vโขwโขฮดโˆ’)โŸaโˆ’โขฯˆ2โขโˆ’(2โขvโขwโขฮดโˆ’โข(ฮธ+ฯ€))โŸbโˆ’โขฯˆabsentsubscriptโŸ๐‘ฃ๐‘คsuperscript๐›ฟsuperscript๐‘Žsuperscript๐œ“2subscriptโŸ2๐‘ฃ๐‘คsuperscript๐›ฟ๐œƒ๐œ‹superscript๐‘๐œ“\displaystyle=\underbrace{(vw\delta^{-})}_{a^{-}}\psi^{2}\underbrace{-(2vw%\delta^{-}(\theta+\pi))}_{b^{-}}\psi= underโŸ start_ARG ( italic_v italic_w italic_ฮด start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ) end_ARG start_POSTSUBSCRIPT italic_a start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT end_POSTSUBSCRIPT italic_ฯˆ start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT underโŸ start_ARG - ( 2 italic_v italic_w italic_ฮด start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ( italic_ฮธ + italic_ฯ€ ) ) end_ARG start_POSTSUBSCRIPT italic_b start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT end_POSTSUBSCRIPT italic_ฯˆ(21)
+(vโˆ’w+vโขwโขฮดโˆ’โข(ฮธ+ฯ€)2)โŸcโˆ’,subscriptโŸ๐‘ฃ๐‘ค๐‘ฃ๐‘คsuperscript๐›ฟsuperscript๐œƒ๐œ‹2superscript๐‘\displaystyle\qquad\qquad+\underbrace{(v-w+vw\delta^{-}(\theta+\pi)^{2})}_{c^{%-}}\;,+ underโŸ start_ARG ( italic_v - italic_w + italic_v italic_w italic_ฮด start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ( italic_ฮธ + italic_ฯ€ ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ) end_ARG start_POSTSUBSCRIPT italic_c start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT end_POSTSUBSCRIPT ,(22)

where ฮดโˆ’=1/v2โˆ’2โขvโขw+w2superscript๐›ฟ1superscript๐‘ฃ22๐‘ฃ๐‘คsuperscript๐‘ค2\delta^{-}=1/\sqrt{v^{2}-2vw+w^{2}}italic_ฮด start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT = 1 / square-root start_ARG italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - 2 italic_v italic_w + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT end_ARG. Since ฮด+<ฮดโˆ’superscript๐›ฟsuperscript๐›ฟ\delta^{+}<\delta^{-}italic_ฮด start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT < italic_ฮด start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT the curvature at the minimum is greater. Let ฯˆ0+superscriptsubscript๐œ“0\psi_{0}^{+}italic_ฯˆ start_POSTSUBSCRIPT 0 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT denote a guess for the heading angle of the maximum. Applying a second-order polynomial least-square fit to the the data on the interval [ฯˆ0+โˆ’ฮป,ฯˆ0++ฮป]superscriptsubscript๐œ“0๐œ†superscriptsubscript๐œ“0๐œ†[\psi_{0}^{+}-\lambda,\psi_{0}^{+}+\lambda][ italic_ฯˆ start_POSTSUBSCRIPT 0 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT - italic_ฮป , italic_ฯˆ start_POSTSUBSCRIPT 0 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT + italic_ฮป ] to give the coefficients (a+,b+,c+)superscript๐‘Žsuperscript๐‘superscript๐‘(a^{+},b^{+},c^{+})( italic_a start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT , italic_b start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT , italic_c start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT ). Equating these coefficients to the Taylor series approximation one can solve for an estimate (v^,w^,ฮธ^)^๐‘ฃ^๐‘ค^๐œƒ(\hat{v},\hat{w},\hat{\theta})( over^ start_ARG italic_v end_ARG , over^ start_ARG italic_w end_ARG , over^ start_ARG italic_ฮธ end_ARG ). A similar approach at the minimum gives the coefficients (aโˆ’,bโˆ’,cโˆ’)superscript๐‘Žsuperscript๐‘superscript๐‘(a^{-},b^{-},c^{-})( italic_a start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT , italic_b start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT , italic_c start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ) and another estimate. To utilize more of the available data, both sets of parameters can be used. Let the coordinate of the maximum and minimum be

(ฯˆmax,vmax)=(โˆ’b+/(2โขa+),c+โˆ’(b+)2/(4โขa+))subscript๐œ“maxsubscript๐‘ฃmaxsuperscript๐‘2superscript๐‘Žsuperscript๐‘superscriptsuperscript๐‘24superscript๐‘Ž\displaystyle(\psi_{\rm max},v_{\rm max})=(-b^{+}/(2a^{+}),~{}c^{+}-(b^{+})^{2%}/(4a^{+}))( italic_ฯˆ start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT , italic_v start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT ) = ( - italic_b start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT / ( 2 italic_a start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT ) , italic_c start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT - ( italic_b start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT / ( 4 italic_a start_POSTSUPERSCRIPT + end_POSTSUPERSCRIPT ) )(23)
(ฯˆmin,vmin)=(โˆ’bโˆ’/(2โขaโˆ’),cโˆ’โˆ’(bโˆ’)2/(4โขaโˆ’)),subscript๐œ“minsubscript๐‘ฃminsuperscript๐‘2superscript๐‘Žsuperscript๐‘superscriptsuperscript๐‘24superscript๐‘Ž\displaystyle(\psi_{\rm min},v_{\rm min})=(-b^{-}/(2a^{-}),~{}c^{-}-(b^{-})^{2%}/(4a^{-}))\;,( italic_ฯˆ start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT , italic_v start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT ) = ( - italic_b start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT / ( 2 italic_a start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ) , italic_c start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT - ( italic_b start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ) start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT / ( 4 italic_a start_POSTSUPERSCRIPT - end_POSTSUPERSCRIPT ) ) ,(24)

respectively.Then, the estimates are:

v^^๐‘ฃ\displaystyle\hat{v}over^ start_ARG italic_v end_ARG=(vmax+vmin)/2absentsubscript๐‘ฃmaxsubscript๐‘ฃmin2\displaystyle=(v_{\rm max}+v_{\rm min})/2= ( italic_v start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT + italic_v start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT ) / 2(25)
w^^๐‘ค\displaystyle\hat{w}over^ start_ARG italic_w end_ARG=(vmaxโˆ’vmin)/2absentsubscript๐‘ฃmaxsubscript๐‘ฃmin2\displaystyle=(v_{\rm max}-v_{\rm min})/2= ( italic_v start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT - italic_v start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT ) / 2(26)
ฮธ^^๐œƒ\displaystyle\hat{\theta}over^ start_ARG italic_ฮธ end_ARG=atan2โข(sinโกฯˆmaxโˆ’sinโกฯˆmin,cosโกฯˆmaxโˆ’cosโกฯˆmin)absentatan2subscript๐œ“maxsubscript๐œ“minsubscript๐œ“maxsubscript๐œ“min\displaystyle={\rm atan2}(\sin\psi_{\rm max}-\sin\psi_{\rm min},\cos\psi_{\rmmax%}-\cos\psi_{\rm min})= atan2 ( roman_sin italic_ฯˆ start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT - roman_sin italic_ฯˆ start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT , roman_cos italic_ฯˆ start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT - roman_cos italic_ฯˆ start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT )(27)

The estimate ฮธ^^๐œƒ\hat{\theta}over^ start_ARG italic_ฮธ end_ARG in (27) averages ฯˆminโˆ’ฯ€subscript๐œ“min๐œ‹\psi_{\rm min}-\piitalic_ฯˆ start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT - italic_ฯ€ and ฯˆmaxsubscript๐œ“max\psi_{\rm max}italic_ฯˆ start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT while considering angle wrap-around. The approach applied to example data is shown in Fig.3.

3 Optimization-Based Methods

This section proposes two optimization-based approaches to estimate the vehicle speed and current magnitude and direction. The first method applies to (xห™,yห™,ฯˆ)ห™๐‘ฅห™๐‘ฆ๐œ“(\dot{x},\dot{y},\psi)( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) data and the second method applies to (vg,ฯˆ)subscript๐‘ฃg๐œ“(v_{\rm g},\psi)( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT , italic_ฯˆ ) data.

3.1 Least-square Optimization with (xห™,yห™,ฯˆ)ห™๐‘ฅห™๐‘ฆ๐œ“(\dot{x},\dot{y},\psi)( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) Data

Suppose that the vehicle obtains noise corrupted data of the form {xห™i,yห™i,ฯˆi}i=1Nsuperscriptsubscriptsubscriptห™๐‘ฅ๐‘–subscriptห™๐‘ฆ๐‘–subscript๐œ“๐‘–๐‘–1๐‘\{\dot{x}_{i},\dot{y}_{i},\psi_{i}\}_{i=1}^{N}{ overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT } start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT and consider the cost function:

J๐ฝ\displaystyle Jitalic_J=12โขโˆ‘i=1N{[xห™iโˆ’fxโข(ฯˆi;v,w,ฮธ)]2+[yห™iโˆ’fyโข(ฯˆi;v,w,ฮธ)]2}absent12superscriptsubscript๐‘–1๐‘superscriptdelimited-[]subscriptห™๐‘ฅ๐‘–subscript๐‘“๐‘ฅsubscript๐œ“๐‘–๐‘ฃ๐‘ค๐œƒ2superscriptdelimited-[]subscriptห™๐‘ฆ๐‘–subscript๐‘“๐‘ฆsubscript๐œ“๐‘–๐‘ฃ๐‘ค๐œƒ2\displaystyle=\frac{1}{2}\sum_{i=1}^{N}\{[\dot{x}_{i}-f_{x}(\psi_{i};v,w,%\theta)]^{2}+[\dot{y}_{i}-f_{y}(\psi_{i};v,w,\theta)]^{2}\}= divide start_ARG 1 end_ARG start_ARG 2 end_ARG โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT { [ overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_f start_POSTSUBSCRIPT italic_x end_POSTSUBSCRIPT ( italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ; italic_v , italic_w , italic_ฮธ ) ] start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + [ overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_f start_POSTSUBSCRIPT italic_y end_POSTSUBSCRIPT ( italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ; italic_v , italic_w , italic_ฮธ ) ] start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT }
=12โˆ‘i=1N{v2+w2+xห™i2+yห™i2โˆ’2xห™icosฮธโˆ’2wyห™isinฮธ\displaystyle=\frac{1}{2}\sum_{i=1}^{N}\left\{v^{2}+w^{2}+\dot{x}_{i}^{2}+\dot%{y}_{i}^{2}-2\dot{x}_{i}\cos\theta-2w\dot{y}_{i}\sin\theta\right.= divide start_ARG 1 end_ARG start_ARG 2 end_ARG โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT { italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - 2 overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT roman_cos italic_ฮธ - 2 italic_w overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT roman_sin italic_ฮธ
โˆ’2โขvโขxห™iโขcosโกฯˆiโˆ’2โขvโขyห™iโขsinโกฯˆi+2โขvโขwโขcosโกฮธโขcosโกฯˆi2๐‘ฃsubscriptห™๐‘ฅ๐‘–subscript๐œ“๐‘–2๐‘ฃsubscriptห™๐‘ฆ๐‘–subscript๐œ“๐‘–2๐‘ฃ๐‘ค๐œƒsubscript๐œ“๐‘–\displaystyle\qquad-2v\dot{x}_{i}\cos\psi_{i}-2v\dot{y}_{i}\sin\psi_{i}+2vw%\cos\theta\cos\psi_{i}- 2 italic_v overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT roman_cos italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - 2 italic_v overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT roman_sin italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT + 2 italic_v italic_w roman_cos italic_ฮธ roman_cos italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT
+2vwsinฮธsinฯˆi}.\displaystyle\qquad\left.+2vw\sin\theta\sin\psi_{i}\right\}\;.+ 2 italic_v italic_w roman_sin italic_ฮธ roman_sin italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT } .(28)

Now define the following constants that depend on the data collected:

k1:=โˆ‘i=1Nxห™i,k2:=โˆ‘i=1Nyห™i,k3:=โˆ‘i=1Nxห™i2formulae-sequenceassignsubscript๐‘˜1superscriptsubscript๐‘–1๐‘subscriptห™๐‘ฅ๐‘–formulae-sequenceassignsubscript๐‘˜2superscriptsubscript๐‘–1๐‘subscriptห™๐‘ฆ๐‘–assignsubscript๐‘˜3superscriptsubscript๐‘–1๐‘superscriptsubscriptห™๐‘ฅ๐‘–2\displaystyle k_{1}:=\sum_{i=1}^{N}\dot{x}_{i}\;,\quad k_{2}:=\sum_{i=1}^{N}%\dot{y}_{i}\;,\quad k_{3}:=\sum_{i=1}^{N}\dot{x}_{i}^{2}italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , italic_k start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , italic_k start_POSTSUBSCRIPT 3 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT
k4:=โˆ‘i=1Nyห™i2,k5:=โˆ‘i=1Ncosโกฯˆi,k6:=โˆ‘i=1Nsinโกฯˆiformulae-sequenceassignsubscript๐‘˜4superscriptsubscript๐‘–1๐‘superscriptsubscriptห™๐‘ฆ๐‘–2formulae-sequenceassignsubscript๐‘˜5superscriptsubscript๐‘–1๐‘subscript๐œ“๐‘–assignsubscript๐‘˜6superscriptsubscript๐‘–1๐‘subscript๐œ“๐‘–\displaystyle k_{4}:=\sum_{i=1}^{N}\dot{y}_{i}^{2}\;,\quad k_{5}:=\sum_{i=1}^{%N}\cos\psi_{i}\;,\quad k_{6}:=\sum_{i=1}^{N}\sin\psi_{i}italic_k start_POSTSUBSCRIPT 4 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT , italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT roman_cos italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT roman_sin italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT
k7:=โˆ‘i=1Nxห™iโขcosโกฯˆi,k8:=โˆ‘i=1Nyห™iโขsinโกฯˆi.formulae-sequenceassignsubscript๐‘˜7superscriptsubscript๐‘–1๐‘subscriptห™๐‘ฅ๐‘–subscript๐œ“๐‘–assignsubscript๐‘˜8superscriptsubscript๐‘–1๐‘subscriptห™๐‘ฆ๐‘–subscript๐œ“๐‘–\displaystyle k_{7}:=\sum_{i=1}^{N}\dot{x}_{i}\cos\psi_{i}\;,\quad k_{8}:=\sum%_{i=1}^{N}\dot{y}_{i}\sin\psi_{i}\;.italic_k start_POSTSUBSCRIPT 7 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT roman_cos italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , italic_k start_POSTSUBSCRIPT 8 end_POSTSUBSCRIPT := โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT roman_sin italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT .

The cost function is then rewritten as

J๐ฝ\displaystyle Jitalic_J=N22โขv2+N2โขw2+12โขk3+12โขk4โˆ’k1โขwโขcosโกฮธabsentsuperscript๐‘22superscript๐‘ฃ2๐‘2superscript๐‘ค212subscript๐‘˜312subscript๐‘˜4subscript๐‘˜1๐‘ค๐œƒ\displaystyle=\frac{N}{2}^{2}v^{2}+\frac{N}{2}w^{2}+\frac{1}{2}k_{3}+\frac{1}{%2}k_{4}-k_{1}w\cos\theta= divide start_ARG italic_N end_ARG start_ARG 2 end_ARG start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + divide start_ARG italic_N end_ARG start_ARG 2 end_ARG italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + divide start_ARG 1 end_ARG start_ARG 2 end_ARG italic_k start_POSTSUBSCRIPT 3 end_POSTSUBSCRIPT + divide start_ARG 1 end_ARG start_ARG 2 end_ARG italic_k start_POSTSUBSCRIPT 4 end_POSTSUBSCRIPT - italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT italic_w roman_cos italic_ฮธ
โˆ’k2โขwโขsinโกฮธโˆ’k7โขvโˆ’k8โขv+k5โขvโขwโขcosโกฮธ+k6โขvโขwโขsinโกฮธ,subscript๐‘˜2๐‘ค๐œƒsubscript๐‘˜7๐‘ฃsubscript๐‘˜8๐‘ฃsubscript๐‘˜5๐‘ฃ๐‘ค๐œƒsubscript๐‘˜6๐‘ฃ๐‘ค๐œƒ\displaystyle-k_{2}w\sin\theta-k_{7}v-k_{8}v+k_{5}vw\cos\theta+k_{6}vw\sin%\theta\;,- italic_k start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT italic_w roman_sin italic_ฮธ - italic_k start_POSTSUBSCRIPT 7 end_POSTSUBSCRIPT italic_v - italic_k start_POSTSUBSCRIPT 8 end_POSTSUBSCRIPT italic_v + italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_v italic_w roman_cos italic_ฮธ + italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_v italic_w roman_sin italic_ฮธ ,(29)

and notice that (29) is linear in the data-dependent variables k1,โ€ฆ,k8subscript๐‘˜1โ€ฆsubscript๐‘˜8k_{1},\ldots,k_{8}italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT , โ€ฆ , italic_k start_POSTSUBSCRIPT 8 end_POSTSUBSCRIPT. These terms only need to be computed once before the optimization proceeds.The gradient with respect to the parameters is:

โˆ‡J=[โˆ‚J/โˆ‚v,โˆ‚J/โˆ‚w,โˆ‚J/โˆ‚ฮธ]T,โˆ‡๐ฝsuperscriptdelimited-[]๐ฝ๐‘ฃ๐ฝ๐‘ค๐ฝ๐œƒT\displaystyle\nabla J=\left[\begin{array}[]{ccc}{\partial J}/{\partial v},&{%\partial J}/{\partial w},&{\partial J}/{\partial\theta}\end{array}\right]^{\rmT%}\;,โˆ‡ italic_J = [ start_ARRAY start_ROW start_CELL โˆ‚ italic_J / โˆ‚ italic_v , end_CELL start_CELL โˆ‚ italic_J / โˆ‚ italic_w , end_CELL start_CELL โˆ‚ italic_J / โˆ‚ italic_ฮธ end_CELL end_ROW end_ARRAY ] start_POSTSUPERSCRIPT roman_T end_POSTSUPERSCRIPT ,(31)

where

โˆ‚J/โˆ‚v๐ฝ๐‘ฃ\displaystyle{\partial J}/{\partial v}โˆ‚ italic_J / โˆ‚ italic_v=Nโขvโˆ’k7โˆ’k8+k5โขwโขcosโกฮธ+k6โขwโขsinโกฮธabsent๐‘๐‘ฃsubscript๐‘˜7subscript๐‘˜8subscript๐‘˜5๐‘ค๐œƒsubscript๐‘˜6๐‘ค๐œƒ\displaystyle=Nv-k_{7}-k_{8}+k_{5}w\cos\theta+k_{6}w\sin\theta= italic_N italic_v - italic_k start_POSTSUBSCRIPT 7 end_POSTSUBSCRIPT - italic_k start_POSTSUBSCRIPT 8 end_POSTSUBSCRIPT + italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_w roman_cos italic_ฮธ + italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_w roman_sin italic_ฮธ(32)
โˆ‚J/โˆ‚w๐ฝ๐‘ค\displaystyle{\partial J}/{\partial w}โˆ‚ italic_J / โˆ‚ italic_w=Nโขw+(k5โขvโˆ’k1)โขcosโกฮธ+(k6โขvโˆ’k2)โขsinโกฮธabsent๐‘๐‘คsubscript๐‘˜5๐‘ฃsubscript๐‘˜1๐œƒsubscript๐‘˜6๐‘ฃsubscript๐‘˜2๐œƒ\displaystyle=Nw+(k_{5}v-k_{1})\cos\theta+(k_{6}v-k_{2})\sin\theta= italic_N italic_w + ( italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_v - italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT ) roman_cos italic_ฮธ + ( italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_v - italic_k start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT ) roman_sin italic_ฮธ(33)
โˆ‚J/โˆ‚ฮธ๐ฝ๐œƒ\displaystyle{\partial J}/{\partial\theta}โˆ‚ italic_J / โˆ‚ italic_ฮธ=(k1โˆ’k5โขv)โขwโขsinโกฮธ+(k6โขvโˆ’k2)โขwโขcosโกฮธ.absentsubscript๐‘˜1subscript๐‘˜5๐‘ฃ๐‘ค๐œƒsubscript๐‘˜6๐‘ฃsubscript๐‘˜2๐‘ค๐œƒ\displaystyle=(k_{1}-k_{5}v)w\sin\theta+(k_{6}v-k_{2})w\cos\theta\;.= ( italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT - italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_v ) italic_w roman_sin italic_ฮธ + ( italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_v - italic_k start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT ) italic_w roman_cos italic_ฮธ .(34)

The symmetric Hessian matrix is:

๐‘ฏ=[HvโขvHvโขwHvโขฮธHwโขvHwโขwHwโขฮธHฮธโขvHฮธโขwHฮธโขฮธ]๐‘ฏdelimited-[]subscript๐ป๐‘ฃ๐‘ฃsubscript๐ป๐‘ฃ๐‘คsubscript๐ป๐‘ฃ๐œƒsubscript๐ป๐‘ค๐‘ฃsubscript๐ป๐‘ค๐‘คsubscript๐ป๐‘ค๐œƒsubscript๐ป๐œƒ๐‘ฃsubscript๐ป๐œƒ๐‘คsubscript๐ป๐œƒ๐œƒ{\bm{H}}=\left[\begin{array}[]{ccc}H_{vv}&H_{vw}&H_{v\theta}\\H_{wv}&H_{ww}&H_{w\theta}\\H_{\theta v}&H_{\theta w}&H_{\theta\theta}\end{array}\right]bold_italic_H = [ start_ARRAY start_ROW start_CELL italic_H start_POSTSUBSCRIPT italic_v italic_v end_POSTSUBSCRIPT end_CELL start_CELL italic_H start_POSTSUBSCRIPT italic_v italic_w end_POSTSUBSCRIPT end_CELL start_CELL italic_H start_POSTSUBSCRIPT italic_v italic_ฮธ end_POSTSUBSCRIPT end_CELL end_ROW start_ROW start_CELL italic_H start_POSTSUBSCRIPT italic_w italic_v end_POSTSUBSCRIPT end_CELL start_CELL italic_H start_POSTSUBSCRIPT italic_w italic_w end_POSTSUBSCRIPT end_CELL start_CELL italic_H start_POSTSUBSCRIPT italic_w italic_ฮธ end_POSTSUBSCRIPT end_CELL end_ROW start_ROW start_CELL italic_H start_POSTSUBSCRIPT italic_ฮธ italic_v end_POSTSUBSCRIPT end_CELL start_CELL italic_H start_POSTSUBSCRIPT italic_ฮธ italic_w end_POSTSUBSCRIPT end_CELL start_CELL italic_H start_POSTSUBSCRIPT italic_ฮธ italic_ฮธ end_POSTSUBSCRIPT end_CELL end_ROW end_ARRAY ](35)

with entries

Hvโขvsubscript๐ป๐‘ฃ๐‘ฃ\displaystyle H_{vv}italic_H start_POSTSUBSCRIPT italic_v italic_v end_POSTSUBSCRIPT=Nabsent๐‘\displaystyle=N= italic_N
Hvโขwsubscript๐ป๐‘ฃ๐‘ค\displaystyle H_{vw}italic_H start_POSTSUBSCRIPT italic_v italic_w end_POSTSUBSCRIPT=k5โขcosโกฮธ+k6โขsinโกฮธabsentsubscript๐‘˜5๐œƒsubscript๐‘˜6๐œƒ\displaystyle=k_{5}\cos\theta+k_{6}\sin\theta= italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT roman_cos italic_ฮธ + italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT roman_sin italic_ฮธ
Hvโขฮธsubscript๐ป๐‘ฃ๐œƒ\displaystyle H_{v\theta}italic_H start_POSTSUBSCRIPT italic_v italic_ฮธ end_POSTSUBSCRIPT=k6โขwโขcosโกฮธโˆ’k5โขwโขsinโกฮธabsentsubscript๐‘˜6๐‘ค๐œƒsubscript๐‘˜5๐‘ค๐œƒ\displaystyle=k_{6}w\cos\theta-k_{5}w\sin\theta= italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_w roman_cos italic_ฮธ - italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_w roman_sin italic_ฮธ
Hwโขwsubscript๐ป๐‘ค๐‘ค\displaystyle H_{ww}italic_H start_POSTSUBSCRIPT italic_w italic_w end_POSTSUBSCRIPT=Nabsent๐‘\displaystyle=N= italic_N
Hwโขฮธsubscript๐ป๐‘ค๐œƒ\displaystyle H_{w\theta}italic_H start_POSTSUBSCRIPT italic_w italic_ฮธ end_POSTSUBSCRIPT=(k1โˆ’k5โขv)โขsinโกฮธ+(k6โขvโˆ’k2)โขcosโกฮธabsentsubscript๐‘˜1subscript๐‘˜5๐‘ฃ๐œƒsubscript๐‘˜6๐‘ฃsubscript๐‘˜2๐œƒ\displaystyle=(k_{1}-k_{5}v)\sin\theta+(k_{6}v-k_{2})\cos\theta= ( italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT - italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_v ) roman_sin italic_ฮธ + ( italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_v - italic_k start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT ) roman_cos italic_ฮธ
Hฮธโขฮธsubscript๐ป๐œƒ๐œƒ\displaystyle H_{\theta\theta}italic_H start_POSTSUBSCRIPT italic_ฮธ italic_ฮธ end_POSTSUBSCRIPT=(k1โˆ’k5โขv)โขwโขcosโกฮธ+(k2โˆ’k6โขv)โขwโขsinโกฮธabsentsubscript๐‘˜1subscript๐‘˜5๐‘ฃ๐‘ค๐œƒsubscript๐‘˜2subscript๐‘˜6๐‘ฃ๐‘ค๐œƒ\displaystyle=(k_{1}-k_{5}v)w\cos\theta+(k_{2}-k_{6}v)w\sin\theta= ( italic_k start_POSTSUBSCRIPT 1 end_POSTSUBSCRIPT - italic_k start_POSTSUBSCRIPT 5 end_POSTSUBSCRIPT italic_v ) italic_w roman_cos italic_ฮธ + ( italic_k start_POSTSUBSCRIPT 2 end_POSTSUBSCRIPT - italic_k start_POSTSUBSCRIPT 6 end_POSTSUBSCRIPT italic_v ) italic_w roman_sin italic_ฮธ

where Hwโขv=Hvโขwsubscript๐ป๐‘ค๐‘ฃsubscript๐ป๐‘ฃ๐‘คH_{wv}=H_{vw}italic_H start_POSTSUBSCRIPT italic_w italic_v end_POSTSUBSCRIPT = italic_H start_POSTSUBSCRIPT italic_v italic_w end_POSTSUBSCRIPT, Hฮธโขv=Hvโขฮธsubscript๐ป๐œƒ๐‘ฃsubscript๐ป๐‘ฃ๐œƒH_{\theta v}=H_{v\theta}italic_H start_POSTSUBSCRIPT italic_ฮธ italic_v end_POSTSUBSCRIPT = italic_H start_POSTSUBSCRIPT italic_v italic_ฮธ end_POSTSUBSCRIPT, and Hฮธโขw=Hwโขฮธsubscript๐ป๐œƒ๐‘คsubscript๐ป๐‘ค๐œƒH_{\theta w}=H_{w\theta}italic_H start_POSTSUBSCRIPT italic_ฮธ italic_w end_POSTSUBSCRIPT = italic_H start_POSTSUBSCRIPT italic_w italic_ฮธ end_POSTSUBSCRIPT.

The cost, gradient, and Hessian are essential for optimization (e.g., via Newton-Rhapsonโ€™s method). By deriving the analytical forms of the gradient and Hessian the use of finite difference techniques can be avoided to improve optimization efficiency and robustness. In this work, we supply โˆ‡Jโˆ‡๐ฝ\nabla Jโˆ‡ italic_J and H๐ปHitalic_H to an interior-point optimizer implemented by the fmincon function in MATLAB. A constrained optimization problem is formulated subject to the linear inequality constraints

[โˆ’100100010โˆ’11000100โˆ’1]โข[vwฮธ]โ‰ค[vminvmax0002โขฯ€],delimited-[]100100010110001001delimited-[]๐‘ฃ๐‘ค๐œƒdelimited-[]subscript๐‘ฃminsubscript๐‘ฃmax0002๐œ‹\left[\begin{array}[]{ccc}-1&0&0\\1&0&0\\0&1&0\\-1&1&0\\0&0&1\\0&0&-1\\\end{array}\right]\left[\begin{array}[]{c}v\\w\\\theta\end{array}\right]\leq\left[\begin{array}[]{c}v_{\rm min}\\v_{\rm max}\\0\\0\\0\\2\pi\\\end{array}\right]\;,[ start_ARRAY start_ROW start_CELL - 1 end_CELL start_CELL 0 end_CELL start_CELL 0 end_CELL end_ROW start_ROW start_CELL 1 end_CELL start_CELL 0 end_CELL start_CELL 0 end_CELL end_ROW start_ROW start_CELL 0 end_CELL start_CELL 1 end_CELL start_CELL 0 end_CELL end_ROW start_ROW start_CELL - 1 end_CELL start_CELL 1 end_CELL start_CELL 0 end_CELL end_ROW start_ROW start_CELL 0 end_CELL start_CELL 0 end_CELL start_CELL 1 end_CELL end_ROW start_ROW start_CELL 0 end_CELL start_CELL 0 end_CELL start_CELL - 1 end_CELL end_ROW end_ARRAY ] [ start_ARRAY start_ROW start_CELL italic_v end_CELL end_ROW start_ROW start_CELL italic_w end_CELL end_ROW start_ROW start_CELL italic_ฮธ end_CELL end_ROW end_ARRAY ] โ‰ค [ start_ARRAY start_ROW start_CELL italic_v start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT end_CELL end_ROW start_ROW start_CELL italic_v start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT end_CELL end_ROW start_ROW start_CELL 0 end_CELL end_ROW start_ROW start_CELL 0 end_CELL end_ROW start_ROW start_CELL 0 end_CELL end_ROW start_ROW start_CELL 2 italic_ฯ€ end_CELL end_ROW end_ARRAY ] ,(36)

where vminsubscript๐‘ฃminv_{\rm min}italic_v start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT and vmaxsubscript๐‘ฃmaxv_{\rm max}italic_v start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT are user-supplied lower and upper bounds on the vehicleโ€™s flow-relative speed. The constraints (36) also encode for the assumption wโ‰คv๐‘ค๐‘ฃw\leq vitalic_w โ‰ค italic_v that is required for the problem to be well posed. Local minima can occur along the boundaries of the constraint set since the topology of ฮธ๐œƒ\thetaitalic_ฮธ is not considered in (36). Additionally, local minima may occur with noisier datasets or when the maneuver contains a smaller range of heading angles. To improve robustness the optimization is run from multiple start points and the lowest cost solution is selected. The initial starting points are selected as the corners of the polytope represented by the constraints and as the centroid of the polytope. An example of the cost function and optimization scheme is shown in Fig.4. The level sets of the cost function are indicative of the uncertainty in the parameter space around the estimated point.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (4)

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (5)

3.2 Least-square Optimization with (vg,ฯˆ)subscript๐‘ฃg๐œ“(v_{\rm g},\psi)( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT , italic_ฯˆ ) Data

Lastly, we consider an optimization-based estimation algorithm that assumes speed-over-ground measurements and heading data are available. Consider the cost function

J๐ฝ\displaystyle Jitalic_J=12โขโˆ‘i=1N[(vg)iโˆ’fvgโข(ฯˆ;v,w,ฮธ)]2.absent12superscriptsubscript๐‘–1๐‘superscriptdelimited-[]subscriptsubscript๐‘ฃg๐‘–subscript๐‘“subscript๐‘ฃg๐œ“๐‘ฃ๐‘ค๐œƒ2\displaystyle=\frac{1}{2}\sum_{i=1}^{N}[(v_{\rm g})_{i}-f_{v_{\rm g}}(\psi;v,w%,\theta)]^{2}\;.= divide start_ARG 1 end_ARG start_ARG 2 end_ARG โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT [ ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_f start_POSTSUBSCRIPT italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT end_POSTSUBSCRIPT ( italic_ฯˆ ; italic_v , italic_w , italic_ฮธ ) ] start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT .(37)

The gradient with respect to the parameters has the same form as (31) with entries

โˆ‚Jโˆ‚v๐ฝ๐‘ฃ\displaystyle\frac{\partial J}{\partial v}divide start_ARG โˆ‚ italic_J end_ARG start_ARG โˆ‚ italic_v end_ARG=โˆ’โˆ‘i=1N(2โขv+2โขwโข(cฯˆโขฮธ)i)โข((vg)iโˆ’ฮฑi)ฮฑiabsentsuperscriptsubscript๐‘–1๐‘2๐‘ฃ2๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–subscript๐›ผ๐‘–subscript๐›ผ๐‘–\displaystyle=-\sum_{i=1}^{N}\frac{\left(2v+2w(c_{\psi\theta})_{i}\right)\left%((v_{\rm g})_{i}-\sqrt{\alpha_{i}}\right)}{\sqrt{\alpha_{i}}}= - โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT divide start_ARG ( 2 italic_v + 2 italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG ) end_ARG start_ARG square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG end_ARG(38)
โˆ‚Jโˆ‚w๐ฝ๐‘ค\displaystyle\frac{\partial J}{\partial w}divide start_ARG โˆ‚ italic_J end_ARG start_ARG โˆ‚ italic_w end_ARG=โˆ’โˆ‘i=1N(2โขw+2โขvโข(cฯˆโขฮธ)i)โข((vg)iโˆ’ฮฑi)ฮฑiabsentsuperscriptsubscript๐‘–1๐‘2๐‘ค2๐‘ฃsubscriptsubscript๐‘๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–subscript๐›ผ๐‘–subscript๐›ผ๐‘–\displaystyle=-\sum_{i=1}^{N}\frac{\left(2w+2v(c_{\psi\theta})_{i}\right)\left%((v_{\rm g})_{i}-\sqrt{\alpha_{i}}\right)}{\sqrt{\alpha_{i}}}= - โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT divide start_ARG ( 2 italic_w + 2 italic_v ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG ) end_ARG start_ARG square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG end_ARG(39)
โˆ‚Jโˆ‚ฮธ๐ฝ๐œƒ\displaystyle\frac{\partial J}{\partial\theta}divide start_ARG โˆ‚ italic_J end_ARG start_ARG โˆ‚ italic_ฮธ end_ARG=โˆ’โˆ‘i=1N2โขvโขwโข(sฯˆโขฮธ)iโข((vg)iโˆ’ฮฑi)ฮฑi,absentsuperscriptsubscript๐‘–1๐‘2๐‘ฃ๐‘คsubscriptsubscript๐‘ ๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–subscript๐›ผ๐‘–subscript๐›ผ๐‘–\displaystyle=-\sum_{i=1}^{N}\frac{2vw(s_{\psi\theta})_{i}\left((v_{\rm g})_{i%}-\sqrt{\alpha_{i}}\right)}{\sqrt{\alpha_{i}}}\;,= - โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT divide start_ARG 2 italic_v italic_w ( italic_s start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG ) end_ARG start_ARG square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG end_ARG ,(40)

where ฮฑi=(v2+2โขcosโก(ฯˆiโˆ’ฮธ)โขvโขw+w2)subscript๐›ผ๐‘–superscript๐‘ฃ22subscript๐œ“๐‘–๐œƒ๐‘ฃ๐‘คsuperscript๐‘ค2\alpha_{i}=\left(v^{2}+2\cos\left(\psi_{i}-\theta\right)vw+w^{2}\right)italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT = ( italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + 2 roman_cos ( italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_ฮธ ) italic_v italic_w + italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ).The symmetric Hessian matrix is the same as in (35)with entries

Hvโขvsubscript๐ป๐‘ฃ๐‘ฃ\displaystyle H_{vv}italic_H start_POSTSUBSCRIPT italic_v italic_v end_POSTSUBSCRIPT=โˆ‘i=1N2โข(ฮฑi3/2โˆ’(vg)iโขw2+(vg)iโขw2โข(cฯˆโขฮธ)i2)ฮฑi3/2absentsuperscriptsubscript๐‘–1๐‘2superscriptsubscript๐›ผ๐‘–32subscriptsubscript๐‘ฃg๐‘–superscript๐‘ค2subscriptsubscript๐‘ฃg๐‘–superscript๐‘ค2superscriptsubscriptsubscript๐‘๐œ“๐œƒ๐‘–2superscriptsubscript๐›ผ๐‘–32\displaystyle=\sum_{i=1}^{N}\frac{2\left({\alpha_{i}}^{3/2}-(v_{\rm g})_{i}w^{%2}+(v_{\rm g})_{i}w^{2}{(c_{\psi\theta})_{i}}^{2}\right)}{{\alpha_{i}}^{3/2}}= โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT divide start_ARG 2 ( italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT - ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT + ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ) end_ARG start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT end_ARG
Hvโขwsubscript๐ป๐‘ฃ๐‘ค\displaystyle H_{vw}italic_H start_POSTSUBSCRIPT italic_v italic_w end_POSTSUBSCRIPT=โˆ‘i=1N(2โขv+2โขwโข(cฯˆโขฮธ)i)โข(2โขw+2โขvโข(cฯˆโขฮธ)i)2โขฮฑiabsentsuperscriptsubscript๐‘–1๐‘2๐‘ฃ2๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–2๐‘ค2๐‘ฃsubscriptsubscript๐‘๐œ“๐œƒ๐‘–2subscript๐›ผ๐‘–\displaystyle=\sum_{i=1}^{N}\frac{\left(2v+2w(c_{\psi\theta})_{i}\right)\left(%2w+2v(c_{\psi\theta})_{i}\right)}{2\alpha_{i}}= โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT divide start_ARG ( 2 italic_v + 2 italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) ( 2 italic_w + 2 italic_v ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) end_ARG start_ARG 2 italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG
โˆ’2โข(cฯˆโขฮธ)iโข((vg)iโˆ’ฮฑi)ฮฑi2subscriptsubscript๐‘๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–subscript๐›ผ๐‘–subscript๐›ผ๐‘–\displaystyle\quad-\frac{2(c_{\psi\theta})_{i}\left((v_{\rm g})_{i}-\sqrt{%\alpha_{i}}\right)}{\sqrt{\alpha_{i}}}- divide start_ARG 2 ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG ) end_ARG start_ARG square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG end_ARG
+(2โขv+2โขwโข(cฯˆโขฮธ)i)โข(2โขw+2โขvโข(cฯˆโขฮธ)i)โข((vg)iโˆ’ฮฑi)2โขฮฑi3/22๐‘ฃ2๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–2๐‘ค2๐‘ฃsubscriptsubscript๐‘๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–subscript๐›ผ๐‘–2superscriptsubscript๐›ผ๐‘–32\displaystyle\quad+\frac{\left(2v+2w(c_{\psi\theta})_{i}\right)\left(2w+2v(c_{%\psi\theta})_{i}\right)\left((v_{\rm g})_{i}-\sqrt{\alpha_{i}}\right)}{2{%\alpha_{i}}^{3/2}}+ divide start_ARG ( 2 italic_v + 2 italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) ( 2 italic_w + 2 italic_v ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - square-root start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT end_ARG ) end_ARG start_ARG 2 italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT end_ARG
Hvโขฮธsubscript๐ป๐‘ฃ๐œƒ\displaystyle H_{v\theta}italic_H start_POSTSUBSCRIPT italic_v italic_ฮธ end_POSTSUBSCRIPT=โˆ‘i=1Nโˆ’2โขwโข(sฯˆโขฮธ)iโข((vg)iโขw2โˆ’ฮฑi3/2+vโข(vg)iโขwโข(cฯˆโขฮธ)i)ฮฑi3/2absentsuperscriptsubscript๐‘–1๐‘2๐‘คsubscriptsubscript๐‘ ๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–superscript๐‘ค2superscriptsubscript๐›ผ๐‘–32๐‘ฃsubscriptsubscript๐‘ฃg๐‘–๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–superscriptsubscript๐›ผ๐‘–32\displaystyle=\sum_{i=1}^{N}-\frac{2w(s_{\psi\theta})_{i}\left((v_{\rm g})_{i}%w^{2}-{\alpha_{i}}^{3/2}+v(v_{\rm g})_{i}w(c_{\psi\theta})_{i}\right)}{{\alpha%_{i}}^{3/2}}= โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT - divide start_ARG 2 italic_w ( italic_s start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT + italic_v ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) end_ARG start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT end_ARG
Hwโขwsubscript๐ป๐‘ค๐‘ค\displaystyle H_{ww}italic_H start_POSTSUBSCRIPT italic_w italic_w end_POSTSUBSCRIPT=โˆ‘i=1Nโˆ’2โขvโข(sฯˆโขฮธ)iโข(v2โข(vg)iโˆ’ฮฑi3/2+vโข(vg)iโขwโข(cฯˆโขฮธ)i)ฮฑi3/2absentsuperscriptsubscript๐‘–1๐‘2๐‘ฃsubscriptsubscript๐‘ ๐œ“๐œƒ๐‘–superscript๐‘ฃ2subscriptsubscript๐‘ฃg๐‘–superscriptsubscript๐›ผ๐‘–32๐‘ฃsubscriptsubscript๐‘ฃg๐‘–๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–superscriptsubscript๐›ผ๐‘–32\displaystyle=\sum_{i=1}^{N}-\frac{2v(s_{\psi\theta})_{i}\left(v^{2}(v_{\rm g}%)_{i}-{\alpha_{i}}^{3/2}+v(v_{\rm g})_{i}w(c_{\psi\theta})_{i}\right)}{{\alpha%_{i}}^{3/2}}= โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT - divide start_ARG 2 italic_v ( italic_s start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ( italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT + italic_v ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) end_ARG start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT end_ARG
Hwโขฮธsubscript๐ป๐‘ค๐œƒ\displaystyle H_{w\theta}italic_H start_POSTSUBSCRIPT italic_w italic_ฮธ end_POSTSUBSCRIPT=โˆ‘i=1Nโˆ’2โขvโข(sฯˆโขฮธ)iโข(v2โข(vg)iโˆ’ฮฑi3/2+vโข(vg)iโขwโข(cฯˆโขฮธ)i)ฮฑi3/2absentsuperscriptsubscript๐‘–1๐‘2๐‘ฃsubscriptsubscript๐‘ ๐œ“๐œƒ๐‘–superscript๐‘ฃ2subscriptsubscript๐‘ฃg๐‘–superscriptsubscript๐›ผ๐‘–32๐‘ฃsubscriptsubscript๐‘ฃg๐‘–๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–superscriptsubscript๐›ผ๐‘–32\displaystyle=\sum_{i=1}^{N}-\frac{2v(s_{\psi\theta})_{i}\left(v^{2}(v_{\rm g}%)_{i}-{\alpha_{i}}^{3/2}+v(v_{\rm g})_{i}w(c_{\psi\theta})_{i}\right)}{{\alpha%_{i}}^{3/2}}= โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT - divide start_ARG 2 italic_v ( italic_s start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ( italic_v start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT + italic_v ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) end_ARG start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT end_ARG
Hฮธโขฮธsubscript๐ป๐œƒ๐œƒ\displaystyle H_{\theta\theta}italic_H start_POSTSUBSCRIPT italic_ฮธ italic_ฮธ end_POSTSUBSCRIPT=โˆ‘i=1Nโˆ’2โขwโข(sฯˆโขฮธ)iโข((vg)iโขw2โˆ’ฮฑi3/2+vโข(vg)iโขwโข(cฯˆโขฮธ)i)ฮฑi3/2absentsuperscriptsubscript๐‘–1๐‘2๐‘คsubscriptsubscript๐‘ ๐œ“๐œƒ๐‘–subscriptsubscript๐‘ฃg๐‘–superscript๐‘ค2superscriptsubscript๐›ผ๐‘–32๐‘ฃsubscriptsubscript๐‘ฃg๐‘–๐‘คsubscriptsubscript๐‘๐œ“๐œƒ๐‘–superscriptsubscript๐›ผ๐‘–32\displaystyle=\sum_{i=1}^{N}-\frac{2w(s_{\psi\theta})_{i}\left((v_{\rm g})_{i}%w^{2}-{\alpha_{i}}^{3/2}+v(v_{\rm g})_{i}w(c_{\psi\theta})_{i}\right)}{{\alpha%_{i}}^{3/2}}= โˆ‘ start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT - divide start_ARG 2 italic_w ( italic_s start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ( ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w start_POSTSUPERSCRIPT 2 end_POSTSUPERSCRIPT - italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT + italic_v ( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT italic_w ( italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT ) start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT ) end_ARG start_ARG italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT start_POSTSUPERSCRIPT 3 / 2 end_POSTSUPERSCRIPT end_ARG

where cฯˆโขฮธ=cosโก(ฯˆiโˆ’ฮธ)subscript๐‘๐œ“๐œƒsubscript๐œ“๐‘–๐œƒc_{\psi\theta}=\cos\left(\psi_{i}-\theta\right)italic_c start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT = roman_cos ( italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_ฮธ ) and sฯˆโขฮธ=sinโก(ฯˆiโˆ’ฮธ)subscript๐‘ ๐œ“๐œƒsubscript๐œ“๐‘–๐œƒs_{\psi\theta}=\sin\left(\psi_{i}-\theta\right)italic_s start_POSTSUBSCRIPT italic_ฯˆ italic_ฮธ end_POSTSUBSCRIPT = roman_sin ( italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT - italic_ฮธ ) and with Hwโขv=Hvโขwsubscript๐ป๐‘ค๐‘ฃsubscript๐ป๐‘ฃ๐‘คH_{wv}=H_{vw}italic_H start_POSTSUBSCRIPT italic_w italic_v end_POSTSUBSCRIPT = italic_H start_POSTSUBSCRIPT italic_v italic_w end_POSTSUBSCRIPT, Hฮธโขv=Hvโขฮธsubscript๐ป๐œƒ๐‘ฃsubscript๐ป๐‘ฃ๐œƒH_{\theta v}=H_{v\theta}italic_H start_POSTSUBSCRIPT italic_ฮธ italic_v end_POSTSUBSCRIPT = italic_H start_POSTSUBSCRIPT italic_v italic_ฮธ end_POSTSUBSCRIPT, and Hฮธโขw=Hwโขฮธsubscript๐ป๐œƒ๐‘คsubscript๐ป๐‘ค๐œƒH_{\theta w}=H_{w\theta}italic_H start_POSTSUBSCRIPT italic_ฮธ italic_w end_POSTSUBSCRIPT = italic_H start_POSTSUBSCRIPT italic_w italic_ฮธ end_POSTSUBSCRIPT. Both the gradient and the Hessian are well-defined only if ฮฑiโ‰ 0subscript๐›ผ๐‘–0\alpha_{i}\neq 0italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT โ‰  0. In practice, the case ฮฑi=0subscript๐›ผ๐‘–0\alpha_{i}=0italic_ฮฑ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT = 0 occurs rarely and during implementation we check for and remove any such instances from the dataset.

Unlike the previous optimization case, the cost function, gradient, and Hessian depend non-linearly on the data. The summations involving {xห™i,yห™i,ฯˆi}i=1Nsuperscriptsubscriptsubscriptห™๐‘ฅ๐‘–subscriptห™๐‘ฆ๐‘–subscript๐œ“๐‘–๐‘–1๐‘\{\dot{x}_{i},\dot{y}_{i},\psi_{i}\}_{i=1}^{N}{ overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , overห™ start_ARG italic_y end_ARG start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT , italic_ฯˆ start_POSTSUBSCRIPT italic_i end_POSTSUBSCRIPT } start_POSTSUBSCRIPT italic_i = 1 end_POSTSUBSCRIPT start_POSTSUPERSCRIPT italic_N end_POSTSUPERSCRIPT must be recomputed at each iteration. As described earlier, the analytical forms of ฮ”โขJฮ”๐ฝ\Delta Jroman_ฮ” italic_J and H๐ปHitalic_H are provided to fmincon along with constraints (36). An example of the cost function and optimization sequence is shown in Fig.5.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (6)

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (7)

4 Comparison using Simulated Data

To compare the performance of the four methods described in Secs. 2 and 3, a Monte Carlo experiment was conducted using simulated data. The data was simulated for three different noise settings, ฯƒ={0.01,0.05,0.10}๐œŽ0.010.050.10\sigma=\{0.01,0.05,0.10\}italic_ฯƒ = { 0.01 , 0.05 , 0.10 } m/s, and three different heading angle changes, ฮ”โขฯˆ={2โขฯ€,3โขฯ€/2,ฯ€}ฮ”๐œ“2๐œ‹3๐œ‹2๐œ‹\Delta\psi=\{2\pi,3\pi/2,\pi\}roman_ฮ” italic_ฯˆ = { 2 italic_ฯ€ , 3 italic_ฯ€ / 2 , italic_ฯ€ }. For each combination of ฯƒ๐œŽ\sigmaitalic_ฯƒ and ฮ”โขฯˆฮ”๐œ“\Delta\psiroman_ฮ” italic_ฯˆ a total of 250 unique datasets of N=100๐‘100N=100italic_N = 100 noisy samples were generated and transformed to the appropriate form for each algorithm. Each dataset was generated by randomizing the true parameters (v,w,ฮธ)๐‘ฃ๐‘ค๐œƒ(v,w,\theta)( italic_v , italic_w , italic_ฮธ ) and drawing them uniformly at random from the interior of the polytope represented by the constraints with vmin=0.5subscript๐‘ฃmin0.5v_{\rm min}=0.5italic_v start_POSTSUBSCRIPT roman_min end_POSTSUBSCRIPT = 0.5 and vmax=5subscript๐‘ฃmax5v_{\rm max}=5italic_v start_POSTSUBSCRIPT roman_max end_POSTSUBSCRIPT = 5 m/s. For each dataset the circular fit algorithm, optimization with (xห™,yห™)ห™๐‘ฅห™๐‘ฆ(\dot{x},\dot{y})( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG ) data, and optimization with (vห™g,ฯˆ)subscriptห™๐‘ฃ๐‘”๐œ“(\dot{v}_{g},\psi)( overห™ start_ARG italic_v end_ARG start_POSTSUBSCRIPT italic_g end_POSTSUBSCRIPT , italic_ฯˆ ) data were tested. The quadratic fit approach was evaluated only for the ฮ”โขฯˆ=2โขฯ€ฮ”๐œ“2๐œ‹\Delta\psi=2\piroman_ฮ” italic_ฯˆ = 2 italic_ฯ€ case. The estimate each algorithm produced was compared to the actual value and the absolute error was recorded. The mean of the absolute error, average over 250 trials, is shown in Fig.6.

As expected, the error for each algorithm increased as the measurement noise is increased and as the range of data is reduced. The quadratic fit method produced the largest errors (e.g., almost 18 deg in mean current direction error for the highest noise setting). The optimization with the (vg,ฯˆ)subscript๐‘ฃg๐œ“(v_{\rm g},\psi)( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT , italic_ฯˆ ) data was not as accurate as the optimization with (xห™,yห™,ฯˆ)ห™๐‘ฅห™๐‘ฆ๐œ“(\dot{x},\dot{y},\psi)( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) data. This is not surprising since more information is available when the components of the ground velocity are known, rather than the magnitude. The circle fit, which uses (xห™,yห™)ห™๐‘ฅห™๐‘ฆ(\dot{x},\dot{y})( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG ) data, in most cases had an intermediate error in comparison to the the two optimization-based methods. The optimization method using (xห™,yห™,ฯˆ)ห™๐‘ฅห™๐‘ฆ๐œ“(\dot{x},\dot{y},\psi)( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) data produced the most accurate results โ€” for example, in the case of data collected for ฮ”โขฯˆ=2โขฯ€ฮ”๐œ“2๐œ‹\Delta\psi=2\piroman_ฮ” italic_ฯˆ = 2 italic_ฯ€ maneuvers the vehicle and current speed errors were <<< 0.01 m/s and the current direction error was <<< 5 degrees.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (8)

5 Comparison using Experimental Data

The four methods described in Secs.2 and 3 were also evaluated using experimental data obtained by a Bluefin-21 unmanned underwater vehicle operated by the U.S. Naval Research Laboratory. The Bluefin-21 is a 20 ft length, 21 inch diameter vehicle developed by Bluefin Robotics that features a fiber-optic gyroscope inertial navigation system and Doppler velocity log (DVL) navigation suite. The vehicle was deployed on June 22, 2016 near Boston Harbor southeast of Nahant Bay. The vehicle was programmed to execute a series of circular orbits around points of interest to test an onboard sonar system. The ground track of the vehicle is shown in Fig.7 along with the flow direction estimated by the optimization method with (xห™,yห™,ฯˆ)ห™๐‘ฅห™๐‘ฆ๐œ“(\dot{x},\dot{y},\psi)( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) data.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (9)

The radii of the smaller and larger orbits were 70 and 120 meters, respectively. The data under analysis was truncated to be within 10 meters of these orbits and input into all four estimation algorithms. An example of the dataset along one such orbit is shown in Fig.8. The parameters estimated by each algorithm were used to superimpose a vgโข(ฯˆ)subscript๐‘ฃg๐œ“v_{\rm g}(\psi)italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT ( italic_ฯˆ ) curve over the data points. The dataset exhibits outliers that do not conform to the model (e.g., due to transient motions as the vehicle enters and exits each orbit). To address this issue the optimization algorithms are wrapped in a random sample consensus (RANSAC) algorithm outer loop.

The estimated current direction and magnitude was also compared to historical data from an instrumented buoy (identifier: BOS1132) deployed by the National Oceanic and Atmospheric Administration (NOAA) located in Stellwagen Bank about 51 km away directly east from the testing area (15nm NNE of Race Point at a depth of 27ft). The results show good agreement with the ebb and flow tidal directions reported by the buoy. The current magnitude matches the general trend and variations may be due to the spatial separation of the buoy and vehicle leading to different tidal flows closer to shore.

Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (10)
Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (11)

6 Conclusion

Three batch estimation methods were presented that determine the direction and magnitude of a steady, uniform, flow-field, and the vehicleโ€™s speed, using noisy kinematic measurements during heading change maneuvers, such as circular orbits or 180 degree turns. The three methods proposed included a quadratic curve fitting approach with (vg,ฯˆ)subscript๐‘ฃg๐œ“(v_{\rm g},\psi)( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT , italic_ฯˆ ) data and least-square optimization methods with either (xห™,โขyห™,ฯˆ)subscriptห™๐‘ฅ,ห™๐‘ฆ๐œ“(\dot{x}_{,}\dot{y},\psi)( overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT , end_POSTSUBSCRIPT overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) or (vg,ฯˆ)subscript๐‘ฃg๐œ“(v_{\rm g},\psi)( italic_v start_POSTSUBSCRIPT roman_g end_POSTSUBSCRIPT , italic_ฯˆ ) data. The methods were compared through a Monte Carlo experiment with simulated data to illustrate the impact of measurement noise and heading angle change during the maneuver. The comparison included an existing circular curve fitting method from the literature that uses (xห™,yห™)ห™๐‘ฅห™๐‘ฆ(\dot{x},\dot{y})( overห™ start_ARG italic_x end_ARG , overห™ start_ARG italic_y end_ARG ) data. The results indicated that the optimization with (xห™,โขyห™,ฯˆ)subscriptห™๐‘ฅ,ห™๐‘ฆ๐œ“(\dot{x}_{,}\dot{y},\psi)( overห™ start_ARG italic_x end_ARG start_POSTSUBSCRIPT , end_POSTSUBSCRIPT overห™ start_ARG italic_y end_ARG , italic_ฯˆ ) led to the lowest mean errors. The methods were also evaluated using a experimental data obtained by an underwater vehicle performing a series of circular orbits during a tidal change. The estimated values had modest agreement with data recorded by a nearby buoy.

The advantages of the proposed methods are that they are relatively simple to implement, depend on only a small number of parameters, and do not require a vehicle model or assumption of vehicle flow-relative speed. The optimization methods also allow visualizing the uncertainty in the parameter space as level sets of a corresponding cost function. Future work may consider running the estimators onboard a vehicle, comparing to dynamic state estimators (e.g., Kalman filters that estimate flow-field conditions), and using flow-field estimates to optimize mission plans.

References

  • Chang etal. [2017]Chang, D., Wu, W., Edwards, C.R., and Zhang, F. (2017).Motion tomography: Mapping flow fields using autonomous underwatervehicles.The International Journal of Robotics Research, 36(3),320โ€“336.
  • Hegrenaes and Hallingstad [2011]Hegrenaes, ร˜. and Hallingstad, O. (2011).Model-aided INS with sea current estimation for robust underwaternavigation.IEEE Journal of Oceanic Engineering, 36(2), 316โ€“337.
  • McLaren [2008]McLaren, S.A. (2008).Velocity estimate following air data system failure.Masterโ€™s thesis, Air Force Institute of Technology.
  • Rhudy etal. [2015]Rhudy, M.B., Fravolini, M.L., Gu, Y., Napolitano, M.R., Gururajan, S., andChao, H. (2015).Aircraft model-independent airspeed estimation without pitot tubemeasurements.IEEE Transactions on Aerospace and Electronic Systems, 51(3),1980โ€“1995.
  • Wolek etal. [2021]Wolek, A., McMahon, J., Dzikowicz, B.R., and Houston, B.H. (2021).The orbiting Dubins traveling salesman problem: planning inspectiontours for a minehunting AUV.Autonomous Robots, 45(1), 31โ€“49.
Batch Estimation of a Steady, Uniform, Flow-Field from Ground Velocity and Heading Measurements (2024)
Top Articles
Latest Posts
Article information

Author: Mrs. Angelic Larkin

Last Updated:

Views: 6370

Rating: 4.7 / 5 (67 voted)

Reviews: 82% of readers found this page helpful

Author information

Name: Mrs. Angelic Larkin

Birthday: 1992-06-28

Address: Apt. 413 8275 Mueller Overpass, South Magnolia, IA 99527-6023

Phone: +6824704719725

Job: District Real-Estate Facilitator

Hobby: Letterboxing, Vacation, Poi, Homebrewing, Mountain biking, Slacklining, Cabaret

Introduction: My name is Mrs. Angelic Larkin, I am a cute, charming, funny, determined, inexpensive, joyous, cheerful person who loves writing and wants to share my knowledge and understanding with you.