1. Abstract
Usual methods for measuring elevation built-in into modern smartwatches and smart devices are based on fusion of data (using Kalman filters, etc) from IMU (accelerometer) and barometer sensors. These methods are designed to measure steps up and down stairs, hiking activities, and so on. However, using pressure as a useful source of elevation data is not suitable to measure the wave height from a boat. The surface of the wave itself (at least in trochoidal wave model) is the surface of equal pressure. Real waves are more complicated but nevertheless measuring air pressure on their surface to estimate amplitude of the wave would lead to measuring mostly noise.
Calculating elevation (displacement) using double integration of the acceleration leads to huge result drift due to not knowing initial displacement, initial vertical velocity and due to signal noise and accelerometer bias.
This article describes two different methods to estimate wave height, frequency, period from a moving boat using IMU (accelerometer) and other common boat sensors such as anemometer (wind), GPS, and compass. One method is based on Kalman method which calculates second integral of acceleration with a drift correction based on the fact that in wave average displacement equals to zero. Another method doesn’t use integration at all. Instead, it measures max/min acceleration and assumes wave shape to be trochoidal. Based on max/min acceleration and observed wave frequency (later adjusted using Doppler effect formulas) the method recreates parameters of the trochoid wave allowing it to estimate the wave height.
Initial velocity and position for Kalman filter are taken as estimates from trochoidal model to allow Kalman filter to converge faster.
2. Trochoidal Waves
Gravitational \$g\$ (m/s^2): \$g = 9.81\$
Rotation center of trochoidal wave particle in Z axis (always negative) (m): \$b\$
Wavelength (m): \$L\$
Wave equation:
\$X(t) = H sin(k c t)\$
\$Z(t) = - H cos(k c t)\$
Wavenumber (1/m): \$k = (2 pi) / L\$
Wave amplitude (m): \$H = e ^(k b) / k = L / (2 pi) e ^ ((2 pi b) / L) \$ ⇒ \$b = ln(H k) / k = L / (2 pi) ln ((2 pi H) / L) \$
max(H) amplitude (m): \$H_max = L / (2 pi)\$ when b = 0
Depth (m): \$d\$
Wave speed (m/s): \$c = sqrt(g / k tanh(k d))\$ , for deep water waves: \$c = sqrt(g / k) = sqrt((g L) / (2 pi))\$, \$L = (g T ^ 2) / (2 pi)\$
Wave period (s): \$T = L / c\$
Wave frequency (Hz=1/s): \$f = 1 / T = c / L\$
Vertical motion is harmonic.
Vertical displacement: \$Z(t) = - H cos(k c t) = - L / (2 pi) (a(t))/g\$
Vertical velocity: \$u(t) = Z^'(t) = k c H sin(k c t) = c e ^ ((2 pi b) / L) sin(k c t)\$
\$u_max = c e ^ ((2 pi b) / L)\$
\$u_max(b=0) = c\$
Vertical acceleration: \$a(t) = u^'(t) = k^2 c^2 H cos(k c t) = g e ^ ((2 pi b) / L) cos(k c t)\$
\$a_max = -a_min = k^2 c^2 H = ((2 pi) / L) ^ 2 ((g L) / (2 pi)) L / (2 pi) e^((2 pi b)/L) = g e^((2 pi b)/L) \$
\$b = L / (2 pi) ln(a_max / g)\$
Slope \$s(t)\$:
\$x_b(t) = X(t) - t Delta v\$
\$s(t) = d/ (dx_b) ((Z(t)) / (x_b(t))) = (d/dt(Z)) / (d/dt(x_b)) = (d/dt(- H cos(kct))) / (d/dt(H sin(kct) - t Delta v)) = (cHk sin(kct)) / (cHk cos(kct) - Delta v)\$
2.1. Doppler effect
\$f\$ is emitted frequency (Hz=1/s),
\$f_o\$ is observed frequency (Hz=1/s):
\$c\$ is the propagation speed of waves in the medium
\$Delta v\$ is the opposite of the velocity of the receiver relative to the source: it is positive when the source and the receiver are moving towards each other.
\$f_o = (c +- Delta v) / c f\$
\$L\$ is source wavelength (m) for trochoidal wave.
For trochoidal wave:
\$f = c / L\$ ⇒ \$f_o = (c +- Delta v) / L \$,
\$c = sqrt((g L) / (2 pi))\$ ⇒ \$f_0 = (sqrt((g L) / (2 pi)) +- Delta v) / L\$
\$L = (sign(Delta v) sqrt(8 pi f_o g Delta v + g ^ 2) + 4 pi f_o Delta v + g) / (4 pi f_o ^ 2)\$
Apparent (observed) vertical acceleration:
\$a_(observed)(Delta v, t) = d^2/dt^2 (- H cos(k (c + Delta v) t)) = H k^2 (c^2 + 2 c Delta v + (Delta v)^2) cos(k (c + Delta v) t)\$
\$a_(observed_max) = H k^2 (c^2 + 2 c Delta v + (Delta v)^2)\$ ⇒
\$H = H(L, Delta v) = a_(observed_max) / (k^2 (c^2 + 2 c Delta v + (Delta v)^2)) \$
\$a_max = a_(observed_max) c^2 / (c^2 + 2 c Delta v + (Delta v)^2)\$
3. Trigonometry of a boat movement
3.1. Terminology
AWS is Apparent Wind Speed (relative to the boat heading)
AWA is Apparent Wind Angle (relative to the bow heading, 0 to 180, starboard plus, port minus)
AWD is Apparent Wind Direction (relative to true north)
AGWS is Apparent Ground Wind Speed (relative to the boat course over the ground)
AGWA is Apparent Ground Wind Angle (relative to the boat course over the ground, 0 to 180, starboard plus, port minus)
AGWD is Apparent Ground Wind Direction (relative to true north)
SPD is Knotmeter speed (relative to the water)
HDT is Heading true (relative to true north)
HDM is Heading magnetic (relative to magnetic north)
DFT is Current Drift (speed of current, relative to fixed earth)
SET is Current Set (direction current flows toward relative to fixed earth true north)
SOG is Speed Over Ground (relative to the fixed earth)
COGT is Course Over Ground true (relative to the fixed earth true north)
COGM is Course Over Ground magnetic (relative to the fixed earth magnetic north)
GWS is Ground Wind Speed (relative to the fixed earth)
GWD is Ground Wind Direction (relative to true north)
TWA is True Wind Angle (relative to the heading, 0 = upwind, 180deg = downwind, (+ starboard, - port))
TWS is True Wind Speed (relative to the water)
TWD is True Wind Direction (relative to true north)
POS is position LAT, LON (latitude, longitude)
TB(POS1, POS2) is Bearing true (true north angle to maintain in course to reach from POS1 to POS2)
3.2. Calculating true wind and speed through water based on difference in heading and course over ground
Having both true heading and true course over the ground allows calculating true wind vector parameters and speed through the water (SPD):
\$SPD = ((dist(POS2, POS1)) / (t_\text{end} - t_\text{start}) - DFT * cos(COGT - SET)) * cos(COGT - avg(HDT))\$
\$TWS = sqrt((avg(AWS)) ^ 2 + SPD ^ 2 - 2 * avg(AWS) * SPD * cos(avg(AWA)))\$
\$TWA = +- arccos((avg(AWS) * cos(avg(AWA)) - SPD) / (TWS))\$
For Doppler effect formulas:
\$Delta v = SPD * cos(TWA)\$
3.3. Heading True from Magnetic
IMU helps measuring magnetic heading HDM. To calculate true heading, it is required to have Earth magnetic model and using location and time it gives you magnetic variation.
import wmm2020
yeardec = datetime.date.today().year
alt_km = 0
declination = wmm2020.wmm(glat, glon, alt_km, yeardec).decl3.4. Leeway
Although leeway calculation is not required for the algorithm (it gets taken into account with using heading to course over ground difference), for completeness, here is the popular empirical formula:
Leeway (deg) is angle to adjust heading to maintain constant COG (assuming no current)
\$\l\eeway = heel K / (SPD ^ 2)\$
\$heel\$ is boat heel and needs to be in (degrees) for the formula above
\$SPD\$ is speed through water and needs to be in (kt)
\$K\$ is boat and boat load specific constant (kt^2), about 10.0
4. IMU
4.1. Getting vertical acceleration from IMU
Most IMU libraries provide a way to get the attitude quaternion \$q\$. Using this quaternion to get acceleration in Earth XYZ from IMU frame xyz: \$\text{accel} = (a_x, a_y, a_z)\$ vector, the quaternion \$q\$ is used to rotate vector \$\text{accel}\$
\$\text{accel}_\text{earthXYZ} = q * \text{accel} * q'\$
where \$q'\$ is inverted quaternion \$q\$.
The same quaternion can be used to get Euler angles of pitch, roll and yaw. Yaw would correspond to the magnetic heading.
| To estimate max acceleration it’s not needed to extract vertical part of acceleration. Just magnitude of acceleration \$a=sqrt(a_x^2 + a_y^2 + a_z^2)\$ should be sufficient. |
4.2. AHRS (attitude and heading reference system) fusion algorithms
If quaternion from IMU is not available, then it can be calculated using AHRS fusion algorithms:
-
Madgwick algorithm
-
Mahony algorithm
-
RTQF quaternion fusion for lower power processors
-
Extended Kalman Filter (EKF)
5. Kalman filter to double integrate vertical acceleration into displacement without integration drift
Ref:
Sharkh S. M., Hendijanizadeh2 M., Moshrefi-Torbati3 M., Abusara M. A.: A Novel Kalman Filter Based Technique for Calculating the Time History of Vertical Displacement of a Boat from Measured Acceleration, Marine Engineering Frontiers (MEF) Volume 2, 2014
The method uses assumption that average vertical displacement is zero. So it takes third integral of acceleration as input measurement with zero value. Vertical acceleration on each step is the value used in transition offset.
So only input observation value is 0 (third integral of acceleration). Acceleration used in transition offset needs to exclude Earth gravitational g and accelerometer bias. So avg(acceleration) is subtracted from it.
Accelerometer noise will make the filter to converge slower. Accelerometer bias will skew the displacement on one side. Giving too low covariances will flatten the result too much. Too high covariances will make it jump too much.
Low pass filter is needed to get rid of high frequency noise before running this filter to get better results.
Kalman method parameters.
Transition matrix:
\$ F = [[1, dt, 1/2 dt ^ 2 ], [0, 1, dt], [0, 0, 1]] \$
Transition offset:
\$ B = ( (1/6 dt^3), (1/2 dt^2), (dt) ) \$
Observation matrix:
\$ H = (1, 0, 0) \$
Transition covariance:
\$ Q = [[ \text{PosIntegral_Trans_Variance}, 0, 0 ], [0, \text{Pos_Trans_Variance}, 0 ], [0, 0, \text{Vel_Trans_Variance} ]] \$
Observation covariance:
\$ R = [[ \text{PosIntegral_Variance} ]] \$
Initial state mean (height integral, height, velocity):
\$ X0 = ((0), (\text{height_from_trochoid_model}), (\text{velocity_from_trochoid_model})) \$
Initial state covariance:
\$ P0 = [[ \text{PosIntegral_Variance}, 0, 0], [0, 1, 0], [0, 0, 1]] \$
from pykalman import KalmanFilter
import numpy as np
kf = KalmanFilter(transition_matrices=F,
observation_matrices=H,
transition_covariance=Q,
observation_covariance=R,
initial_state_mean=X0,
initial_state_covariance=P0)
for t in range(n_timesteps):
if t == 0:
filtered_state_means[t] = X0
filtered_state_covariances[t] = P0
else:
observation = 0
transition_offset = B * (accZ_val[t] - acc_avg)
filtered_state_means[t], filtered_state_covariances[t] = (
kf.filter_update(
filtered_state_mean = filtered_state_means[t-1],
filtered_state_covariance = filtered_state_covariances[t-1],
transition_offset = transition_offset,
observation = observation
)
)The wave height needs to be evaluated based on the results on the end side of the sampling interval. The results in the beginning of the sample interval will have extra spikes due to Kalman filter taking time to converge (without knowing initial velocity and position and adjusting covariances).
5.1. FFT to calculate observed frequency of waves
Fast (discrete) Fourier Transform (FFT) is used to find main frequency (the one which carries most energy) of the waves.
import numpy as np
from scipy import fft
SAMPLE_RATE = 1.0 / dt
N_SAMP = n_timesteps
w = fft.rfft(accZ_val)
freqs = fft.rfftfreq(N_SAMP, 1 / SAMPLE_RATE)
# Find the peak in the coefficients
idx = np.argmax(np.divide(np.divide(np.abs(w), freqs), freqs))
freq = freqs[idx]
freq_in_hertz = abs(freq)
period = 1 / freq_in_hertz5.2. Butterworth Low Pass filter
Butterworth Low pass filter is used to filter out high frequencies noise in input to Kalman filter
import numpy as np
from scipy import signal
sos = signal.butter(2, freq_in_hertz * 8, 'low', fs=SAMPLE_RATE, output='sos')
low_pass_filtered = signal.sosfilt(sos, accZ_val)5.3. Aranovskiy on-line frequency estimator
Instead of FFT method for finding main wave frequency we could use Aranovskiy frequency estimator which is a simple on-line filter.
Ref:
Alexey A. Bobtsov, Nikolay A. Nikolaev, Olga V. Slita, Alexander S. Borgul, Stanislav V. Aranovskiy
The New Algorithm of Sinusoidal Signal Frequency Estimation.
11th IFAC International Workshop on Adaptation and Learning in Control and Signal Processing July 3-5, 2013. Caen, France
from math import sin, sqrt
import matplotlib.pyplot as plt
import numpy as np
# initialize algorithm constants
omega_up = 0.0 # upper frequency
a = 1.0
b = 1.0
k = 1.0
theta_0 = - omega_up * omega_up / 4.0
x1_0 = 0.0
sigma_0 = theta_0
delta_t = 0.001 # time step
count = 30000 # iterations
def func_y(t):
return 2.0 * sin(2.0 * t)
# initialize variables
t = 0.0
x1 = x1_0
theta = theta_0
sigma = sigma_0
chart_x = np.zeros(count)
chart_y = np.zeros(count)
# loop
for n in range(0, count):
# step
y = func_y(t)
x1_dot = - a * x1 + b * y
sigma_dot = - k * x1 * x1 * theta - k * a * x1 * x1_dot - k * b * x1_dot * y
theta = sigma + k * b * x1 * y
omega = sqrt(abs(theta))
print(omega)
chart_x[n] = t
chart_y[n] = omega
x1 = x1 + x1_dot * delta_t
sigma = sigma + sigma_dot * delta_t
t = t + delta_t
plt.plot(chart_x, chart_y)
plt.grid()
plt.show()6. On measuring wave direction
Measuring wave direction can be achieved by using multiple accelerometers mounted in different locations on a ship.
Ref:
Ocean Engineering Volume 249, 1 April 2022, 110760
Johann A. Dirdala, Roger Skjetneb, Jan Roháčc, Thor I. Fossen
Online wave direction and wave number estimation from surface vessel motions using distributed inertial measurement arrays and phase-time-path-differences
7. Measuring wave height from a moving boat
7.1. Measurable input parameters
Open sea waves have periods 20 sec or even longer. So sample duration time should be in minutes to capture several waves. Playing with reference data without noise shows described Kalman algorithm needs about 10 wavelength to converge accurately. So with longer way samples needs to be longer. 5-15 mins.
Acceleration positive ⇒ position negative. They are in counter phase. This could allow setting initial position for Kalman filter better.
Pitch negative ⇒ speed vector is pointing down. This could also allow setting initial velocity for Kalman filter better.
So for initial position and speed in Kalman filter:
\$pos_\text{init} = - a_\text{init} / (k^2 c^2) = - a_\text{init} / (g k) = - L a_\text{init} / (2 pi g) \$
\$tan(\p\i\tch_\text{init}) = const_1 * s_\text{init}(Delta v = 0)\$ tangent of pitch is proportional to wave slope
\$vel_\text{init} = kcH cos(kct) * s_\text{init}(Delta v = 0) = (kcH) / (const_1) sin(\p\itch_\text{init}) = const_2 * c * sin(\p\itch_\text{init}) \$,
\$const_2\$ to be found empirically (it’s vessel specific too).
Or
\$vel_\text{init} = c e^((2 pi b)/L) sin (arccos (a_\text{init} / g e^(-(2 pi b)/ L))) * sign(\p\itch_\text{init}) \$
-
t_start, t_end - time interval of measurements (about 5 mins)
-
POS(t) as LAT(t), LON(t)
-
AWA(t) AWS(t)
-
COG(t) SOG(t)
-
HDM(t) + mag_variation → HDT(t)
-
DFT(t) SET(t) - (possibly from current/tide stations harmonics data)
-
heel(t), pitch(t)
-
SPD(t) - possibly (might be missing) ⇒ leeway(heel(t), SPD(t))
-
accel(t, x, y, z), vertical_accel(t) via pitch,roll,yaw
-
ROT(t) - rate of turn
7.2. Assumptions
-
No tacks, jibes during sample
-
Heading and course over ground are mostly steady
-
Wind is not changing drastically
-
Depth is deep, i.e. depth > 1/2 wavelength
-
Water current is not changing much (or zero i.e. negligible relative to boat speed)
-
Trochoidal wave model
-
Wave length is longer than boat length
-
Approx formula for b, H (in trochoidal wave model)
-
Accelerometer installed near vessel’s center of mass (center of buoyancy)
-
Vessel’s geometry and RAO (Response amplitude operator) is not taken into account
7.3. Algorithm steps
Calculation steps:
-
FFT to get observed wave frequency from acceleration (f_observed)
-
Speed toward wave fronts (delta_v for Doppler frequency) from wind and speed data
-
COGT as true bearing from POS1 to POS2
-
Convert HDM to HDT using position and local mag declination, Use avg(HDT) vs COG and coordinates to calculate SPD
-
SPD = (DIST(POS1, POS2)/(t_end - t_start) - (DFT * cos(COGT - SET))) * cos(COGT - avg(HDT))
-
avg(leeway(heel(t), SPD))
-
use avg(AWA), AVG(AWS) and SPD to calculate TWS/TWA
-
TWS = sqrt(AVG(AWS) ^ 2 + SPD ^ 2 - 2 * AVG(AWS) * SPD * cos(avg(AWA)))
-
TWA = +- arccos( ( AVG(AWS) * cos(avg(AWA)) - SPD ) / TWS)
-
calculate delta_v as SPD * cos(TWA)
-
-
Calculate L_source (source wave length) for trochoidal wave model from f_observed and delta_v using Doppler formulas
-
Low pass filter for accel data
-
min/max accel after low pass
-
Calculate b value for trochoidal wave model from known L_source and min/max accel after low pass
-
Calculate wave height from b and L_source
7.4. Input validation
-
Validate heading and COG for steadiness
-
Validate that magnetic variation is known for the position
-
Check validity of accel (against g)
-
Validate input data against assumptions
-
Discard the sample if it is not good and start collecting a new one.
7.5. Result validation
-
Validate wave amplitude max \$H < H_max = L / (2 pi)\$
-
Validate accel (Ratio to \$g\$ can’t be unreasonably high)
-
Discard the sample if the result is not good and start collecting a new one.
-
Validate height calculated by Kalman filter against height calculated from max acceleration and wavelength with trochoidal model.
8. Further ideas
Trochoidal model gives a good reference point. It’s easy to go from acceleration to vertical displacement in trochoidal model knowing the wavelength. So (I think) it could be a good idea to use trochoidal model estimations as input to Kalman filter fusion (instead of zero third integral assumption), as another way to prevent displacement integral of acceleration from drift.
Vertical velocity and heel are correlated. It would be interesting to study their interdependencies.
9. Implementation
As of September 9th, 2024 author provides complete implementation on GitHub
10. References
-
Sharkh S. M., Hendijanizadeh2 M., Moshrefi-Torbati3 M., Abusara M. A.: A Novel Kalman Filter Based Technique for Calculating the Time History of Vertical Displacement of a Boat from Measured Acceleration, Marine Engineering Frontiers (MEF) Volume 2, 2014
-
Alexey A. Bobtsov, Nikolay A. Nikolaev, Olga V. Slita, Alexander S. Borgul, Stanislav V. Aranovskiy: The New Algorithm of Sinusoidal Signal Frequency Estimation. 11th IFAC International Workshop on Adaptation and Learning in Control and Signal Processing July 3-5, 2013. Caen, France: The New Algorithm of Sinusoidal Signal Frequency Estimation