Discrete Kalman
Discrete Kalman
Discrete Kalman
https://doi.org/10.1016/j.oceaneng.2023.114240
Accepted Version
Level of access, as per info available on SHERPA/ROMEO
http://www.sherpa.ac.uk/romeo/search.php
Discrete-time Kalman filter for heave motion estimation⋆
Joel Reisa , Pedro Batistab , Paulo Oliveirab,c and Carlos Silvestrea,b,∗,1
a Faculty of Science and Technology, University of Macau, Macau, China
b Institutefor Systems and Robotics, Instituto Superior Técnico, Universidade de Lisboa, Lisboa, 1049-001, Portugal
c Associated Laboratory for Energy, Transports and Aeronautics, Institute of Mechanical Engineering, Instituto Superior Técnico, Universidade de
∑
𝑁
( ) ⎡ 𝐂[𝑘0 ] ⎤
𝑎𝑚 [𝑘] − 𝑢[𝑘] = sin 𝜔𝑖 𝑘𝑇𝑠 𝑥1,𝑖 [𝑘] ⎢ 𝐂[𝑘 + 1]𝐀 ⎥
𝑖=1 ⎢ 0 ⎥
(10) O ∶= ⎢ 𝐂[𝑘0 + 2]𝐀2 ⎥ ∈ ℝ(𝓁+1)×(2𝑁+1) . (18)
∑
𝑁
( ) ⎢ ⋮ ⎥
+ cos 𝜔𝑖 𝑘𝑇𝑠 𝑥2,𝑖 [𝑘]. ⎢𝐂[𝑘 + 𝓁]𝐀𝓁 ⎥
𝑖=1 ⎣ 0 ⎦
⎪ ( 𝖳
) 𝖳
1 0.05 0.6853 -0.1834
⎪ K[𝑘] = 𝐀𝐏[𝑘]𝐀 + 𝐐 𝐂[𝑘] × 2 0.1 0.9182 -0.0421
⎨ ( ( ) )−1 3 0.15 0.1323 1.1805
⎪ × R + 𝐂[𝑘] 𝐀𝐏[𝑘]𝐀𝖳 + 𝐐 𝐂[𝑘]𝖳 , (28b)
⎪ ( )( ) 4 0.2 0.5896 -2.9270
𝖳
⎩𝐏[𝑘+1] = 𝐈2𝑁+1 − K[𝑘]𝐂[𝑘] 𝐀𝐏[𝑘]𝐀 + 𝐐 , (28c) 5 0.25 1.3785 -2.5686
where Q ∈ ℝ(2𝑁+1)×(2𝑁+1) , Q ≻ 𝟎, and R > 0, are the co-
variances of the process and observation noises, respectively. where 𝑛𝑢𝑝 (𝑡), 𝑛𝑢𝑟 (𝑡) ∈ ℝ represent another two sequences of
Each of these two covariances, herein assumed constant, is additive white Gaussian noise with standard deviation of 1
associated with a different additive white Gaussian noise degree. The evolution of 𝑢(𝑡𝑘 ) is showcased in Fig. 1.
distribution, and can be seen as a tuning knob adjusted for
10
the best performance.
Finally, an estimate of the heave motion follows as 8
We set out to validate our strategy, particularly by as- Figure 1: Evolution of 𝑢(𝑡𝑘 ) according to (30) and (31).
sessing the performance of the Kalman filter (28) in the
presence of sensor noise, including cross-correlation terms,
as stressed in Remark 1. In regard to the Kalman filter settings, the initial state
Consider a mission scenario where a surface vessel estimate is set to zero, i.e., ̂
𝐱(𝑡𝑘0 = 0) ∶= 𝟎2𝑁+1 . The
equipped with, e.g., a winch system, is subjected to an initial error covariance matrix, and the constant process and
oscillatory motion consisting in the overlay of five waves observations covariances are set (for 𝑁 = 5) as
whose properties are listed in Table 2. Further suppose
𝐏(𝑡𝑘0 = 0) ∶= blkdiag(100𝐈𝑁 , 100𝐈𝑁 , 10−1 ),
that the vessel is equipped with an IMU whose readings
of acceleration are corrupted by additive white Gaussian 𝐐 ∶= blkdiag(10−10 𝐈𝑁 , 10−10 𝐈𝑁 , 10−5 ), (32)
noise with standard deviation of 0.01 m/s2 , as well as by a 𝐑 ∶= 0.01,
sensor offset characterized√by a Brownian motion governed
by 𝑏(𝑡𝑘+1 ) = 𝑏(𝑡𝑘 ) + 0.003 𝑇𝑠 𝑟𝑏 (𝑡𝑘 ), with 𝑏(𝑡𝑘0 = 0) = 0.05 respectively. Finally, the sampling rate is set as 𝑓𝑠 = 50
m/s2 and 𝑟𝑏 ∈ ℝ being a sequence of normally distributed Hz. Note that the matrices in (32) were empirically adjusted
random numbers. We assume that the motion of the sea for best performance, but they still reflect the knowledge of
waves causes the surface vessel to pitch and roll as described model and sensor specifications. In particular, the third entry
by the following parametric functions of time: of 𝐐 is set larger than the remaining ones to allow the filter
( ) to track the time-varying nature of 𝑏(𝑡𝑘 ).
𝜃(𝑡𝑘 ) = 0.92 sin 2𝜋0.033𝑡𝑘 + 2𝜋(𝑟𝑝1 (𝑡𝑘 ) − 0.5) Let us first present the results associated with the esti-
( ) mation error regarding 𝑥1,𝑖 and 𝑥2,𝑖 , for 𝑖 = 1, 2, … , 𝑁,
+ 0.15 sin 2𝜋0.165𝑡𝑘 + 2𝜋(𝑟𝑝2 (𝑡𝑘 ) − 0.5)
( ) plotted in Fig. 2. The evolution of the estimation error
+ 0.53 sin 2𝜋0.32𝑡𝑘 + 2𝜋(𝑟𝑝3 (𝑡𝑘 ) − 0.5) and, associated with the heave is plotted in Fig. 3. Notice the
( ) (30)
𝜑(𝑡𝑘 ) = 0.18 cos 2𝜋0.066𝑡𝑘 + 2𝜋(𝑟𝑟1 (𝑡𝑘 ) − 0.5) filter’s quick convergence, with steady-state behavior being
( ) reached roughly around 10 seconds. The filter’s accuracy in
+ 1.15 cos 2𝜋0.18𝑡𝑘 + 2𝜋(𝑟𝑟2 (𝑡𝑘 ) − 0.5)
( ) steady-state is approximately 4 cm, which is a very good
+ 0.72 cos 2𝜋0.25𝑡𝑘 + 2𝜋(𝑟𝑟3 (𝑡𝑘 ) − 0.5) , result for this kind of application.
where 𝑟𝑝𝑖 , 𝑟𝑟𝑖 ∈ [0, 1], for 𝑖 = 1, 2, 3, are uniformly The estimation over time of each of the five wave am-
distributed random numbers, with 𝜃(𝑡) and 𝜑(𝑡) representing plitudes and five wave phases is showcased in Figs. 4 and 5,
the pitch and roll angles, respectively, such that, according respectively. In line with the results shown in Figs. 2 and 3,
to (6), we have once more the quick convergence is evidenced by the short
( ) ( ) transient period, which lasts roughly 10 seconds. Most in-
𝑢(𝑡𝑘 ) = 𝑔 cos 𝜃(𝑡) + 𝑛𝑢𝑝 (𝑡) cos 𝜑(𝑡) + 𝑛𝑢𝑟 (𝑡) , (31) terestingly, we observe that the frequency associated to each
20 50
0
0
-20
-50
Phase (deg)
-40
-100
40
-150
20
-200
0
-250
-20
-40 -300
0 5 10 15 20 25 30 0 5 10 15 20 25 30
Time (s) Time (s)
Figure 2: Evolution of estimation errors 𝑥̃1,𝑖 (𝑡𝑘 ) ∶= 𝑥1,𝑖 (𝑡𝑘 ) − Figure 5: Evolution of the phase estimates.
̂1,𝑖 (𝑡𝑘 ) and 𝑥
𝑥 ̂2,𝑖 (𝑡𝑘 ), for 𝑖 = 1, 2, … , 𝑁 .
̃2,𝑖 (𝑡𝑘 ) ∶= 𝑥2,𝑖 (𝑡𝑘 ) − 𝑥
Dashed lines correspond to the evolution of the +∕− standard
deviation of the errors.
Note that the filter is able to track relatively fast changes with
15 an accuracy typically less than 10%.
40
10
30
5
20
Error (m)
Amplitude (mg)
0 10
0
-5 0.04
0.02
0
-0.02 -10
-0.04 7.5
-10 20 22 24 26 28 30
-20 7
-15 6.5
-30
0 5 10 15 20 25 30
Time (s) 6
20 22 24 26 28 30
-40
0 5 10 15 20 25 30
Figure 3: Evolution of heave error 𝑧̃(𝑡𝑘 ) ∶= 𝑧(𝑡𝑘 ) − 𝑧̂(𝑡𝑘 ). Time (s)
10 1
Figure 7: Steady-state mean and standard deviation vs. gap Assumption 2. The 𝑁 wave frequencies comprise most
between frequencies 𝑓1 = 0.05 Hz and 𝑓2 = 𝑓1 + Δ𝑓 Hz. of the energy of the continuous wave spectrum. Therefore,
according to (1) and (5), we establish the approximation:
0.7
𝝎 ∶= 𝜔1 𝜔1 + Δ𝜔 𝜔1 + 2Δ𝜔 … 𝜔𝑁 ∈ ℝ𝑁 . (36)
0.6
0.5
Note that this uniform spacing of frequencies reflects the
0.4
fact that no specific knowledge about the wave spectrum is
0.3
obtained a priori.
0.2
We then update the covariance of the process noise in the
0.1
Kalman filter as 𝐐 ∶= blkdiag(10−2 𝐈𝑁 , 10−2 𝐈𝑁 , 10−2 ), and
0
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 we set 𝜔1 = 0.1𝜋 and 𝜔𝑁 = 0.8𝜋. Sensor specifications and
f (Hz)
other simulation parameters remain unchanged.
Figure 8: A simulated example of a Pierson-Moskowitz spec- In Fig. 9 we present the evolution of the resulting heave
trum of ocean waves described by Sea State Code 7. motion associated with (33) and the corresponding ship
dynamics, as well as the evolution of several estimation
Consider as well a large vessel, namely the HMS Nor- sequences obtained for different values of 𝑁 > 50. The
folk, weighing 2500 tonnes, of dimensions 137 × 15 × 16 evolution of the sensor bias and corresponding estimation
m, subjected to the aforementioned waves. The incidence sequences is displayed in Fig. 10.
angle, i.e., the angle between the waves and the direction of As expected, a larger 𝑁 yields more accurate results
motion of the ship was set to 60 degrees. The dynamics of since the whole continuous spectrum is better approximated.
this surface vessel was computed through the Ship Simulator Nevertheless, the overall Kalman filter performance is sat-
available in Granström and Bernat (2021) to obtain realistic isfactory, as evidenced by the calculated root mean square
sequences of pitch and roll, as well as heave motion data. errors listed in the Table 3. Obtaining a similar performance
The region of the spectrum that is of interest is assumed for 𝑁 < 50 is also possible to some extent, although
relatively well-known a priori. In view of the discrete- technically more difficult since the approximation in (34)
time nature of our strategy, we consider that region as an grows weaker. Further tweaking of the initial covariance
interval of 𝑁 discrete frequencies, where, as opposed to the matrix 𝐏, as well as of the constant covariance 𝐑 in (28)
scenario described in Section 7.1, the value of 𝑁 can be may help but with limited efficacy.
arbitrarily chosen relying on an educated guess. This design
trait motivates the following assumption.
-5 0.9
0.8
Normalized Amplitude
-10 0.7
0.6
-15 0.5
0 50 100 150 200 250 300 350
Time (s) 0.4
0.1
25 0
0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4
f (Hz)
20
Figure 11: Continuous Pierson-Moskowitz spectrum alongside
reconstructed discrete spectra based on estimated amplitudes
obtained for dierent values of 𝑁 .
15
Amplitude (mg)
10
8. Conclusions
0
This paper presented a discrete-time linear time-varying
-5
0 50 100 150 200 250 300 350
Kalman filter to solve the problem of heave motion esti-
Time (s) mation using biased accelerometer measurements. Making
Figure 10: Simulated bias drift with Brownian motion and use of convenient and well-known properties of the sin(∙)
corresponding estimation sequences for dierent values of 𝑁 . function, we developed a general framework encapsulating
a linear system whose state encompasses implicit repre-
sentations of wave amplitudes and phase shifts, as well
For the sake of completeness, we display in Fig. 11 a as a constant sensor bias. We showed that the system is
discrete-time normalized reconstruction of the signal spec- observable, which guarantees the Kalman-based estimator
trum for each value of 𝑁. As expected, these discrete se- is globally asymptotically stable. The proposed strategy was
quences closely resemble the continuous spectrum, origi- validated with realistic numerical examples, including an
nally plotted in Fig. 8, apart from some excited modes related accurate representation of a continuous Pierson-Moskowitz
to the motion of the ship. wave spectrum within an ocean context where a large surface
We emphasize the fact that the choice of frequency vessel is subjected to wave motion. Longer missions, as
interval, i.e., 𝜔1 and 𝜔𝑁 , can have an impact on the filter’s well as greater sailed distances, will certainly see the sea
estimation performance, most evidently inferred through the state changing over time, which will cause different spectra
gap in Fig. 11, particularly the empty area to the left of to manifest. Adapting our proposed methodology to this
𝑓 = 0.05 Hz. Allowing 𝝎 to encompass frequencies too scenario will be considered in future work.
close to zero will lead to some ambiguity in the estimation,
as the filter struggles to separate data from low frequencies References
and data from the sensor drift, which is caused by an ill- Auestad, Ø.F., Gravdahl, J.T., Fossen, T.I., 2013. Heave Motion Estimation
conditioned observability matrix. on a Craft Using a Strapdown Inertial Measurement Unit. IFAC Proceed-
Moreover, we stress that the proposed strategy consists ings Volumes 46, 298–303. doi:10.3182/20130918-4-jp-3022.00033.
in a fully discrete-time implementation and that, according Batista, P., Oliveira, P., Silvestre, C., 2018. A Globally Exponentially
to (34), a finite number of frequencies can approximate a Stable Solution for Frequency Estimation, in: 2018 IEEE Conference
on Decision and Control (CDC), IEEE. doi:10.1109/cdc.2018.8619093.
continuous spectrum, but it only does so well when said Ben, Y., Gao, Q., Wei, T., Gong, S., Li, Q., 2022. Real-time heave motion
number is relatively large, and the range defined by 𝜔1 measurement by adaptive band-pass filter based on strapdown INS.
and 𝜔𝑁 encloses the whole spectrum. The frequencies of Ocean Engineering 262, 112278. doi:10.1016/j.oceaneng.2022.112278.
principal excited modes do not, in most cases, coincide with Blake, S.J., 2007. Heave compensation using time-differenced carrier
the selected frequencies, in which case the Kalman filter observations from low cost GPS receivers. Ph.D. thesis. University of
Nottingham.
benefits from of its well adjusted process covariance in order Collins, C.O., Lund, B., Waseda, T., Graber, H.C., 2014. On recording sea
to track slowly-time varying changes: a wrong frequency is surface elevation with accelerometer buoys: lessons from ITOP (2010).
equivalent to an unknown time-varying phase. Ocean Dynamics 64, 895–904. doi:10.1007/s10236-014-0732-7.