├── images ├── .ignore ├── BBN-heave-sensor.jpg ├── Enclosure-for-m5atom.jpg └── BBN-heave-sensor-enclosure.jpg ├── bbn_wave_freq_m5atomS3 ├── data-sim │ ├── sea_reg.sh │ ├── w3d_sim.sh │ ├── freq_track.sh │ ├── qmekf_sim.sh │ ├── w3d_sim_adapt_2.sh │ ├── gen_sim_data.sh │ ├── w3d_sim_adapt_3.sh │ └── Makefile ├── plots │ ├── gen_plots_data.sh │ ├── wave_dir_plots.cpp │ ├── fenton_plots.cpp │ ├── draw_plots.sh │ ├── Makefile │ ├── wave_dir_plots.py │ ├── spectr_sim_plots.py │ ├── fenton_plots.py │ ├── fusion_diag_plots.py │ ├── wave_spectrum_plots.py │ ├── qmekf_plots.py │ ├── wdir_plots.py │ ├── reg_spectra_plots.py │ └── freq_track_plots.py ├── doc │ ├── diag.png │ ├── flow_diag.adoc │ ├── w3d-state.tex-part │ ├── w3d-lti-discrete.tex-part │ ├── w3d-cross-cov.tex-part │ ├── w3d-init.tex-part │ ├── w3d-mnoise.tex-part │ ├── kalman3d_refs.bib │ ├── w3d-ou-q.tex-part │ ├── w3d-phi-symb.tex-part │ ├── w3d-mupdate.tex-part │ ├── sliding_window_min_max.tex │ ├── w3d-dnoise.tex-part │ ├── w3d-phi-A-symb.tex-part │ ├── freq_tracking_zero_crossing.tex │ ├── wave_profile_and_predictor.tex │ ├── ahrs_kalman_qmekf.tex │ ├── w3d-propag.tex-part │ ├── w3d-tupdate.tex-part │ ├── ins_diagram.tex │ ├── w3d-ou12.tex-part │ ├── w3d-proc-symb-assembly.tex-part │ └── freq_tracking_adaptive_notch_kalman.tex ├── tests │ ├── run_tests.sh │ ├── bbn_heave.png │ ├── wave_results.png │ ├── plot_fenton_wave.py │ ├── Makefile │ ├── plot_wave_direction.py │ ├── plot_spectrum.py │ ├── plot_pm_spectrum.py │ ├── plot_results.py │ ├── plot_fenton_wave_tracker.py │ ├── plot_freqs.py │ ├── plot_jonswap.py │ ├── plot_pm_stokes.py │ └── plot_sea_reg.py ├── NmeaChecksum.h ├── symb-math │ └── phi-from-F.py ├── FirstOrderIIRSmoother.h ├── WaveDirectionDetector.h ├── CentripetalAcceleration.h ├── WavesCategories.h ├── EngineVibrationSim.h ├── MinMaxLemire.h ├── KalmanSmoother.h ├── FrequencySmoother.h ├── GPS_to_INS.h ├── TimeAwareSpikeFilter.h ├── WaveFilters.h ├── AranovskiyFilter.h ├── NmeaXDR.h └── Mahony_AHRS.h ├── LICENSE └── bbn_imu_stat_m5atomS3 ├── tests └── results.txt └── bbn_imu_stat_m5atomS3.ino /images/.ignore: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/sea_reg.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | ./sea_reg 4 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/w3d_sim.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | #./w3d_sim 4 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/freq_track.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | ./freq_track 4 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/qmekf_sim.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | ./qmekf_sim 4 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/w3d_sim_adapt_2.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | #./w3d_sim_adapt_2 4 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/gen_plots_data.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | ./fenton_plots 4 | ./wave_dir_plots 5 | -------------------------------------------------------------------------------- /images/BBN-heave-sensor.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bareboat-necessities/bbn-wave-period-esp32/HEAD/images/BBN-heave-sensor.jpg -------------------------------------------------------------------------------- /images/Enclosure-for-m5atom.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bareboat-necessities/bbn-wave-period-esp32/HEAD/images/Enclosure-for-m5atom.jpg -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/diag.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bareboat-necessities/bbn-wave-period-esp32/HEAD/bbn_wave_freq_m5atomS3/doc/diag.png -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/run_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | make clean; make all 4 | ./tests_sea 5 | ./tests_freq 6 | ./tests > results.csv 7 | 8 | -------------------------------------------------------------------------------- /images/BBN-heave-sensor-enclosure.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bareboat-necessities/bbn-wave-period-esp32/HEAD/images/BBN-heave-sensor-enclosure.jpg -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/bbn_heave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bareboat-necessities/bbn-wave-period-esp32/HEAD/bbn_wave_freq_m5atomS3/tests/bbn_heave.png -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/wave_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bareboat-necessities/bbn-wave-period-esp32/HEAD/bbn_wave_freq_m5atomS3/tests/wave_results.png -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/gen_sim_data.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | make clean 4 | make -j4 all 5 | 6 | # run each wave height parallel 7 | seq 0 3 | xargs -n1 -P4 ./waves_sim 8 | #./tunings 9 | #./tunings_est 10 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/w3d_sim_adapt_3.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | #./w3d_sim_adapt_3 --nomag # it seems not possible to make filter stable without using magnetomenter 4 | ./w3d_sim_adapt_3 5 | ./imu_wave_dir 6 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/wave_dir_plots.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2024-2025, Mikhail Grushinskiy 3 | */ 4 | 5 | #define KALMAN_WAVE_DIRECTION_TEST 6 | #define EIGEN_NON_ARDUINO 7 | 8 | #include "KalmanWaveDirection.h" 9 | 10 | int main(int argc, char *argv[]) { 11 | KalmanWaveDirection_test_1(); 12 | } 13 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/fenton_plots.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2024-2025, Mikhail Grushinskiy 3 | */ 4 | 5 | #define FENTON_TEST 6 | #define EIGEN_NON_ARDUINO 7 | 8 | #include "FentonWaveVectorized.h" 9 | 10 | int main(int argc, char *argv[]) { 11 | FentonWave_test_1(); 12 | FentonWave_test_2(); 13 | } 14 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/draw_plots.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | python3 fenton_plots.py 4 | python3 wave_dir_plots.py 5 | python3 wave_sim_plots.py 6 | python3 wave_spectrum_plots.py 7 | python3 freq_track_plots.py 8 | python3 sea_reg_plots.py 9 | python3 qmekf_plots.py 10 | python3 w3d_plots.py 11 | python3 reg_spectra_plots.py 12 | python3 fusion_diag_plots.py 13 | 14 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/Makefile: -------------------------------------------------------------------------------- 1 | CC=g++ 2 | CPPFLAGS=-O3 -march=native -funroll-loops -fno-finite-math-only -I./.. -I/usr/include/eigen3 3 | 4 | .PHONY: all 5 | all: fenton_plots wave_dir_plots 6 | 7 | fenton_plots: fenton_plots.o 8 | $(CC) -o fenton_plots fenton_plots.o 9 | 10 | wave_dir_plots: wave_dir_plots.o 11 | $(CC) -o wave_dir_plots wave_dir_plots.o 12 | 13 | .PHONY: clean 14 | clean: 15 | rm -f *.o fenton_plots fenton_plots.exe wave_dir_plots wave_dir_plots.exe 16 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/NmeaChecksum.h: -------------------------------------------------------------------------------- 1 | #ifndef NmeaChecksum_h 2 | #define NmeaChecksum_h 3 | 4 | #define NMEA_END_CHAR_1 '\r' 5 | 6 | /* 7 | NMEA-0183 checksum 8 | */ 9 | 10 | uint8_t nmea0183_checksum(const char *sentence); 11 | 12 | uint8_t nmea0183_checksum(const char *sentence) { 13 | const char *n = sentence + 1; 14 | uint8_t chk = 0; 15 | /* While current char isn't '*' or sentence ending (newline) */ 16 | while ('*' != *n && NMEA_END_CHAR_1 != *n && '\0' != *n) { 17 | chk ^= (uint8_t) *n; 18 | n++; 19 | } 20 | return chk; 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_fenton_wave.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | 4 | # Load the CSV data 5 | data = pd.read_csv("wave_data.csv") 6 | 7 | # Plot surface elevation η(x) 8 | plt.figure(figsize=(10, 4)) 9 | plt.plot(data['x'], data['elevation'], label='Surface Elevation η(x)', color='blue') 10 | 11 | # Label axes 12 | plt.xlabel("Horizontal Position x (m)") 13 | plt.ylabel("Surface Elevation η (m)") 14 | plt.title("Fenton Wave Profile") 15 | plt.grid(True) 16 | plt.legend() 17 | 18 | # Optional: set aspect ratio 19 | plt.gca().set_aspect('auto', adjustable='box') 20 | 21 | # Show the plot 22 | plt.tight_layout() 23 | plt.show() 24 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/Makefile: -------------------------------------------------------------------------------- 1 | CC=g++ 2 | #CPPFLAGS=-O3 -march=native -fsanitize=address -fno-omit-frame-pointer -funroll-loops -fno-finite-math-only -I./.. -I/usr/include/eigen3 3 | CPPFLAGS=-O3 -funroll-loops -fno-finite-math-only -I./.. -I/usr/include/eigen3 4 | 5 | .PHONY: all 6 | all: tests tests_freq tests_sea 7 | 8 | tests: tests.o 9 | $(CC) $(LDFLAGS) -o $@ $^ 10 | 11 | tests_freq: tests_freq.o 12 | $(CC) $(LDFLAGS) -o $@ $^ 13 | 14 | tests_sea: tests_sea.o 15 | $(CC) $(LDFLAGS) -o $@ $^ 16 | 17 | %.o: %.cpp 18 | $(CC) $(CPPFLAGS) $(CXXFLAGS) -c -o $@ $< 19 | 20 | .PHONY: clean 21 | clean: 22 | rm -f *.o *.csv tests tests.exe tests_freq tests_freq.exe tests_sea tests_sea.exe 23 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/symb-math/phi-from-F.py: -------------------------------------------------------------------------------- 1 | # phi-from-F.py 2 | from sympy import symbols, Matrix, eye, simplify 3 | 4 | def phi_from_F(F, h): 5 | """ 6 | Return the discrete transition Phi(h) = exp(F*h) for a given continuous-time Jacobian F. 7 | Works symbolically or numerically (after substitution). 8 | """ 9 | Phi = (F * h).exp() # matrix exponential 10 | return simplify(Phi) 11 | 12 | # --- Example (delete if you already have F) --- 13 | if __name__ == "__main__": 14 | # Example: tiny 4x4 [v, p, S, a] chain Jacobian (continuous-time) 15 | h, tau = symbols('h tau', positive=True) 16 | F_axis = Matrix([ 17 | [0, 0, 0, 1], # vdot = a 18 | [1, 0, 0, 0], # pdot = v 19 | [0, 1, 0, 0], # Sdot = p 20 | [0, 0, 0, -1/tau] # adot = -(1/tau) a 21 | ]) 22 | 23 | Phi_axis = phi_from_F(F_axis, h) 24 | print("Phi_axis(h) = exp(F_axis*h):") 25 | print(Phi_axis) 26 | 27 | # If you have a full F (e.g., 21x21), just call: 28 | # Phi_full = phi_from_F(F_full, h) 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Bareboat Necessities 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_wave_direction.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | import numpy as np 4 | 5 | df = pd.read_csv("wave_dir.csv") 6 | 7 | t = df["t"] 8 | ax = df["ax"] 9 | ay = df["ay"] 10 | fax = df["filtered_ax"] 11 | fay = df["filtered_ay"] 12 | freq = df["frequency"] 13 | amp = df["amplitude"] 14 | phase = df["phase"] 15 | conf = df["confidence"] 16 | deg = df["deg"] 17 | 18 | # Plot raw and filtered signals 19 | plt.figure(figsize=(12, 8)) 20 | 21 | plt.subplot(4, 1, 1) 22 | plt.plot(t, ax, label="Raw ax", alpha=0.5) 23 | plt.plot(t, fax, label="Filtered ax") 24 | plt.plot(t, ay, label="Raw ay", alpha=0.5) 25 | plt.plot(t, fay, label="Filtered ay") 26 | plt.legend() 27 | plt.title("Raw vs Filtered Signals") 28 | plt.ylabel("Acceleration") 29 | 30 | plt.subplot(4, 1, 2) 31 | plt.plot(t, deg) 32 | plt.title("Estimated Angle") 33 | plt.ylabel("Angle") 34 | 35 | plt.subplot(4, 1, 3) 36 | plt.scatter(ax, ay) 37 | plt.scatter(fax, fay) 38 | plt.title("A_est") 39 | plt.xlabel("X") 40 | plt.ylabel("Y") 41 | 42 | plt.subplot(4, 1, 4) 43 | plt.plot(t, phase) 44 | plt.title("Estimated Phase") 45 | plt.xlabel("Time (s)") 46 | plt.ylabel("Phase (rad)") 47 | 48 | 49 | plt.tight_layout() 50 | plt.show() 51 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/FirstOrderIIRSmoother.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | template 6 | class FirstOrderIIRSmoother { 7 | public: 8 | /** 9 | * @param dt Sample period in seconds (e.g. 1.0f / 240.0f) 10 | * @param settle_time_sec90 Time to reach ~90% of a step (seconds) 11 | */ 12 | FirstOrderIIRSmoother(Real dt, 13 | Real settle_time_sec90) 14 | : y_(Real(0)), 15 | initialized_(false) 16 | { 17 | setGainFromSettleTime(dt, settle_time_sec90); 18 | } 19 | 20 | void setGainFromSettleTime(Real dt, Real settle_time_sec90) { 21 | Real N = settle_time_sec90 / dt; 22 | if (N <= Real(1)) { 23 | K_ = Real(1); // effectively no smoothing 24 | } else { 25 | // K = 1 - 0.1^(1/N) → 90% of step in settle_time_sec90 26 | K_ = Real(1) - std::pow(Real(0.1), Real(1) / N); 27 | } 28 | } 29 | 30 | void setInitial(Real x0) { 31 | y_ = x0; 32 | initialized_ = true; 33 | } 34 | 35 | Real update(Real x) { 36 | if (!initialized_) { 37 | y_ = x; 38 | initialized_ = true; 39 | return y_; 40 | } 41 | y_ += K_ * (x - y_); 42 | return y_; 43 | } 44 | 45 | Real value() const { return y_; } 46 | Real gain() const { return K_; } 47 | 48 | private: 49 | Real y_; 50 | Real K_; 51 | bool initialized_; 52 | }; 53 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/data-sim/Makefile: -------------------------------------------------------------------------------- 1 | CC = g++ 2 | 3 | # Base flags 4 | BASEFLAGS = -O3 -std=c++20 -funroll-loops -fno-finite-math-only -I./.. -I/usr/include/eigen3 5 | 6 | # Detect platform 7 | UNAME_S := $(shell uname -s) 8 | 9 | ifeq ($(UNAME_S),Linux) 10 | CXXFLAGS = $(BASEFLAGS) -march=native 11 | else ifeq ($(UNAME_S),Darwin) # macOS 12 | CXXFLAGS = $(BASEFLAGS) -march=native 13 | else 14 | CXXFLAGS = $(BASEFLAGS) # Windows / Cygwin / others 15 | endif 16 | 17 | LDFLAGS = 18 | 19 | # Targets 20 | TARGETS = imu_wave_dir waves_sim freq_track sea_reg qmekf_sim w3d_sim_adapt_3 spectr_sim # w3d_sim w3d_sim_adapt_2 tunings tunings_est 21 | 22 | all: $(TARGETS) 23 | 24 | waves_sim: waves_sim.o 25 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 26 | 27 | freq_track: freq_track.o 28 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 29 | 30 | sea_reg: sea_reg.o 31 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 32 | 33 | qmekf_sim: qmekf_sim.o 34 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 35 | 36 | w3d_sim: w3d_sim.o 37 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 38 | 39 | w3d_sim_adapt_2: w3d_sim_adapt_2.o 40 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 41 | 42 | w3d_sim_adapt_3: w3d_sim_adapt_3.o 43 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 44 | 45 | tunings: tunings.o 46 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 47 | 48 | tunings_est: tunings_est.o 49 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 50 | 51 | spectr_sim: spectr_sim.o 52 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 53 | 54 | imu_wave_dir: imu_wave_dir.o 55 | $(CC) $(CXXFLAGS) -o $@ $^ $(LDFLAGS) 56 | 57 | %.o: %.cpp 58 | $(CC) $(CXXFLAGS) -c $< -o $@ 59 | 60 | clean: 61 | rm -f *.o $(TARGETS) *.exe *.csv 62 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/WaveDirectionDetector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* 4 | Copyright 2025, Mikhail Grushinskiy 5 | */ 6 | 7 | enum WaveDirection { 8 | BACKWARD = -1, 9 | UNCERTAIN = 0, 10 | FORWARD = 1 11 | }; 12 | 13 | template 14 | class WaveDirectionDetector { 15 | private: 16 | const Real alpha; 17 | const Real threshold; 18 | Real prevVertAccel = NAN; 19 | Real filteredP = Real(0); 20 | 21 | public: 22 | // waveAngle in radians (0=positive X, PI/2=positive Y) 23 | WaveDirectionDetector(Real smoothing = Real(0.002), 24 | Real sensitivity = Real(0.005)) 25 | : alpha(smoothing), 26 | threshold(sensitivity) { 27 | } 28 | 29 | // Processes X,Y,Z accelerations 30 | WaveDirection update(Real accelX, Real accelY, Real accelZ, Real delta_t) { 31 | Real mag_a = sqrtf(accelX * accelX + accelY * accelY); 32 | if (std::isnan(prevVertAccel)) { 33 | prevVertAccel = accelZ; 34 | return UNCERTAIN; 35 | } 36 | if (mag_a > Real(1e-8)) { 37 | // Project X/Y onto wave direction axis 38 | Real aHoriz = accelY > Real(0) ? mag_a : -mag_a; 39 | 40 | // Compute vertical slope 41 | Real vertSlope = (accelZ - prevVertAccel) / delta_t; 42 | prevVertAccel = accelZ; 43 | 44 | // Update EMA filter 45 | filteredP += alpha * (aHoriz * vertSlope - filteredP); 46 | } 47 | 48 | // Threshold decision 49 | if (filteredP > threshold) return FORWARD; 50 | if (filteredP < -threshold) return BACKWARD; 51 | return UNCERTAIN; 52 | } 53 | 54 | Real getFilteredP() const { 55 | return filteredP; 56 | } 57 | }; 58 | 59 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/CentripetalAcceleration.h: -------------------------------------------------------------------------------- 1 | #ifndef CentripetalAcceleration_h 2 | #define CentripetalAcceleration_h 3 | 4 | // Structure to represent a 3D vector 5 | typedef struct { 6 | float x; 7 | float y; 8 | float z; 9 | } Vector3D; 10 | 11 | // Function to compute the cross product of two 3D vectors 12 | Vector3D cross_product(Vector3D a, Vector3D b) { 13 | Vector3D result; 14 | result.x = a.y * b.z - a.z * b.y; 15 | result.y = a.z * b.x - a.x * b.z; 16 | result.z = a.x * b.y - a.y * b.x; 17 | return result; 18 | } 19 | 20 | // Function to subtract two 3D vectors 21 | Vector3D subtract_vectors(Vector3D a, Vector3D b) { 22 | Vector3D result; 23 | result.x = a.x - b.x; 24 | result.y = a.y - b.y; 25 | result.z = a.z - b.z; 26 | return result; 27 | } 28 | 29 | // Function to estimate centripetal acceleration 30 | // Note: the vectors are in the same frame so if 31 | // gyro is in IMU frame of coordinates then 32 | // velocity must br in same device body frame (not Earth xyz frame) 33 | Vector3D estimate_centripetal_acceleration(Vector3D velocity, Vector3D gyro_data) { 34 | // Centripetal acceleration: a_c = ω × v 35 | return cross_product(gyro_data, velocity); 36 | } 37 | 38 | // Function to compensate for centripetal acceleration 39 | Vector3D compensate_centripetal_acceleration(Vector3D accel_data, Vector3D velocity, Vector3D gyro_data) { 40 | // Estimate centripetal acceleration 41 | Vector3D a_c = estimate_centripetal_acceleration(velocity, gyro_data); 42 | 43 | // Subtract centripetal acceleration from accelerometer data 44 | return subtract_vectors(accel_data, a_c); 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/WavesCategories.h: -------------------------------------------------------------------------------- 1 | #ifndef WAVES_CATEGORIES_H 2 | #define WAVES_CATEGORIES_H 3 | 4 | /* 5 | See: https://en.wikipedia.org/wiki/Douglas_sea_scale 6 | 7 | WMO Sea State codes 8 | Height (in meters_): 9 | Calm (glassy) 0 - 0.10 No waves breaking on beach 0 10 | Calm (rippled) 0.10 - 0.1 No waves breaking on beach 1 11 | Smooth 0.1 - 0.5 Slight waves breaking on beach 2 12 | Slight 0.5 - 1.25 Waves rock buoys and small craft 3 13 | Moderate 1.25 - 2.5 Sea becoming furrowed 4 14 | Rough 2.5 - 4 Sea deeply furrowed 5 15 | Very rough 4 - 6 Sea much disturbed with rollers having steep fronts 6 16 | High 6 - 9 Sea much disturbed with rollers having steep fronts (damage to foreshore) 7 17 | Very high 9 - 14 Towering seas 8 18 | Phenomenal 14 - more Precipitous seas (experienced only in cyclones) 9 19 | */ 20 | 21 | typedef enum { 22 | NA = -1, 23 | GLASSY_SEA = 0, 24 | CALM_RIPPLED_SEA = 1, 25 | SMOOTH_SEA = 2, 26 | SLIGHT_WAVES = 3, 27 | MODERATE_WAVES = 4, 28 | ROUGH_SEA = 5, 29 | VERY_ROUGH_SEA = 6, 30 | HIGH_WAVES = 7, 31 | VERY_HIGH_WAVES = 8, 32 | PHENOMENAL_WAVES = 9, 33 | } wave_category_WMO_sea_state_code; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/EngineVibrationSim.h: -------------------------------------------------------------------------------- 1 | #ifndef ENGINE_VIBRATION_SIM_H 2 | #define ENGINE_VIBRATION_SIM_H 3 | 4 | /* 5 | Copyright 2025, Mikhail Grushinskiy 6 | 7 | Engine vibration noise simulation for IMU 8 | */ 9 | 10 | class EngineVibrationSim { 11 | public: 12 | // Constructor with engine RPM and optional gravity (default Earth's gravity) 13 | EngineVibrationSim(float engineRPM, float gravity = 9.80665f) 14 | : rpm(engineRPM), g(gravity), time(0.0f) { 15 | fundamentalFreq = rpm / 60.0f; 16 | } 17 | 18 | // Call every sample with sample period dt in seconds. 19 | // Returns simulated vibration acceleration in m/s² (SI units). 20 | float update(float dt) { 21 | float g_signal = 0.0f; 22 | for (int i = 0; i < numHarmonics; ++i) { 23 | float freq = fundamentalFreq * (i + 1); 24 | g_signal += amps[i] * sinf(2.0f * PI * freq * time); 25 | } 26 | time += dt; 27 | return g_signal * g; // convert g units to m/s² using provided gravity 28 | } 29 | 30 | // Set amplitude of harmonic (index 0..3), amplitude in g 31 | void setHarmonicAmplitude(int index, float amplitude) { 32 | if (index >= 0 && index < numHarmonics) { 33 | amps[index] = amplitude; 34 | } 35 | } 36 | 37 | private: 38 | float rpm; // Engine RPM 39 | float fundamentalFreq; // Fundamental frequency in Hz 40 | float time; // Internal time tracker 41 | float g; // Gravity constant to convert g to m/s² 42 | 43 | static const int numHarmonics = 4; 44 | float amps[numHarmonics] = {0.02f, 0.01f, 0.005f, 0.003f}; // Default amplitudes in g 45 | }; 46 | 47 | #endif // ENGINE_VIBRATION_SIM_H 48 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/MinMaxLemire.h: -------------------------------------------------------------------------------- 1 | #ifndef MinMaxLemire_h 2 | #define MinMaxLemire_h 3 | 4 | #include "MonoWedge.h" 5 | 6 | #include 7 | #include 8 | 9 | typedef struct Sample { 10 | float value; 11 | uint32_t timeMicroSec; 12 | bool operator<(const Sample& o) const { 13 | return value < o.value; 14 | } 15 | bool operator>(const Sample& o) const { 16 | return value > o.value; 17 | } 18 | } SampleType; 19 | 20 | typedef struct min_max_lemire { 21 | Sample min; 22 | Sample max; 23 | std::deque min_wedge; 24 | std::deque max_wedge; 25 | } MinMaxLemire; 26 | 27 | /** 28 | * Running min max 29 | */ 30 | void min_max_lemire_update(MinMaxLemire* minMax, Sample sample, uint32_t window_size_micro_sec); 31 | 32 | void min_max_lemire_update(MinMaxLemire* minMax, Sample sample, uint32_t window_size_micro_sec) { 33 | mono_wedge::max_wedge_update(minMax->max_wedge, sample); 34 | // Get rid of old samples outside our rolling range 35 | while (sample.timeMicroSec - minMax->max_wedge.front().timeMicroSec > window_size_micro_sec) { 36 | auto front = minMax->max_wedge.front(); 37 | minMax->max_wedge.pop_front(); 38 | } 39 | // The maximum value is at the front of the wedge. 40 | auto maximumInRange = minMax->max_wedge.front(); 41 | minMax->max = maximumInRange; 42 | 43 | mono_wedge::min_wedge_update(minMax->min_wedge, sample); 44 | // Get rid of old samples outside our rolling range 45 | while (sample.timeMicroSec - minMax->min_wedge.front().timeMicroSec > window_size_micro_sec) { 46 | auto front = minMax->min_wedge.front(); 47 | minMax->min_wedge.pop_front(); 48 | } 49 | // The minimum value is at the front of the wedge. 50 | auto minimumInRange = minMax->min_wedge.front(); 51 | minMax->min = minimumInRange; 52 | } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /bbn_imu_stat_m5atomS3/tests/results.txt: -------------------------------------------------------------------------------- 1 | gyroX 2 | Count: 2500 3 | Sample Rate: 248.06 4 | Min: -0.1634872 5 | Max: 2.3389542 6 | Average: 0.8456765 7 | variance: 0.0217767 8 | pop stdev: 0.1475692 9 | unbias stdev: 0.1475987 10 | time(micros): 101391659 11 | ===================================== 12 | gyroY 13 | Count: 2500 14 | Sample Rate: 248.03 15 | Min: -3.1673694 16 | Max: 1.7154431 17 | Average: 0.5555643 18 | variance: 0.1505933 19 | pop stdev: 0.3880636 20 | unbias stdev: 0.3881412 21 | time(micros): 101392950 22 | ===================================== 23 | gyroZ 24 | Count: 2500 25 | Sample Rate: 247.99 26 | Min: -1.5730513 27 | Max: 1.0514604 28 | Average: -0.3156822 29 | variance: 0.0235313 30 | pop stdev: 0.1533992 31 | unbias stdev: 0.1534299 32 | time(micros): 101394234 33 | ===================================== 34 | accelX 35 | Count: 2500 36 | Sample Rate: 247.96 37 | Min: 0.2435603 38 | Max: 0.3517146 39 | Average: 0.3047836 40 | variance: 0.0000327 41 | pop stdev: 0.0057168 42 | unbias stdev: 0.0057179 43 | time(micros): 101395465 44 | ===================================== 45 | accelY 46 | Count: 2500 47 | Sample Rate: 247.93 48 | Min: 0.0182141 49 | Max: 0.0509289 50 | Average: 0.0331573 51 | variance: 0.0000062 52 | pop stdev: 0.0024973 53 | unbias stdev: 0.0024978 54 | time(micros): 101396668 55 | ===================================== 56 | accelZ 57 | Count: 2500 58 | Sample Rate: 247.90 59 | Min: 0.9045660 60 | Max: 1.0058844 61 | Average: 0.9558439 62 | variance: 0.0000380 63 | pop stdev: 0.0061658 64 | unbias stdev: 0.0061670 65 | time(micros): 101397881 66 | ===================================== 67 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_spectrum.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from mpl_toolkits.mplot3d import Axes3D 5 | import glob 6 | import os 7 | 8 | # === Find all exported spectrum CSVs === 9 | files = glob.glob("*_jonswap_spectrum.csv") 10 | 11 | if not files: 12 | print("No *_jonswap_spectrum.csv files found in this directory.") 13 | exit() 14 | 15 | for fname in files: 16 | print(f"Processing {fname} ...") 17 | df = pd.read_csv(fname) 18 | 19 | # Extract unique freqs and directions 20 | freqs = np.sort(df["f_Hz"].unique()) 21 | thetas_deg = np.sort(df["theta_deg"].unique()) 22 | M, N = len(thetas_deg), len(freqs) 23 | 24 | # Pivot into 2D grid [freq × theta] 25 | E = df.pivot(index="f_Hz", columns="theta_deg", values="E").values 26 | 27 | # Convert degrees → radians for polar plot 28 | thetas_rad = np.deg2rad(thetas_deg) 29 | 30 | # === 1. Polar Heatmap === 31 | R, T = np.meshgrid(freqs, thetas_rad) # R = frequency, T = direction 32 | fig1, ax1 = plt.subplots(subplot_kw={'projection': 'polar'}) 33 | pcm = ax1.pcolormesh(T, R, E.T, shading='auto', cmap='viridis') 34 | ax1.set_theta_zero_location("N") # 0° at North 35 | ax1.set_theta_direction(-1) # clockwise 36 | ax1.set_title(f"Directional Spectrum\n{os.path.basename(fname)}") 37 | cbar = plt.colorbar(pcm, ax=ax1, orientation="vertical", label="E(f,θ) [m²/Hz/deg]") 38 | 39 | # === 2. 3D Surface Plot === 40 | T_grid, F_grid = np.meshgrid(thetas_deg, freqs) # degrees on axis 41 | fig2 = plt.figure() 42 | ax2 = fig2.add_subplot(111, projection='3d') 43 | surf = ax2.plot_surface(F_grid, T_grid, E, cmap='viridis', linewidth=0, antialiased=True) 44 | ax2.set_xlabel("Frequency [Hz]") 45 | ax2.set_ylabel("Direction [deg]") 46 | ax2.set_zlabel("E(f,θ)") 47 | ax2.set_title(f"Directional Spectrum (3D)\n{os.path.basename(fname)}") 48 | fig2.colorbar(surf, shrink=0.5, aspect=10, label="E(f,θ)") 49 | 50 | plt.show() 51 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_pm_spectrum.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from mpl_toolkits.mplot3d import Axes3D 5 | import glob 6 | import os 7 | 8 | # === Find all exported spectrum CSVs for PM === 9 | files = glob.glob("*_pms_spectrum.csv") 10 | 11 | if not files: 12 | print("No *_pms_spectrum.csv files found in this directory.") 13 | exit() 14 | 15 | for fname in files: 16 | print(f"Processing {fname} ...") 17 | df = pd.read_csv(fname) 18 | 19 | # Extract unique freqs and directions 20 | freqs = np.sort(df["f_Hz"].unique()) 21 | thetas_deg = np.sort(df["theta_deg"].unique()) 22 | M, N = len(thetas_deg), len(freqs) 23 | 24 | # Pivot into 2D grid [freq × theta] 25 | E = df.pivot(index="f_Hz", columns="theta_deg", values="E").values 26 | 27 | # Convert degrees → radians for polar plot 28 | thetas_rad = np.deg2rad(thetas_deg) 29 | 30 | # === 1. Polar Heatmap === 31 | R, T = np.meshgrid(freqs, thetas_rad) # R = frequency, T = direction 32 | fig1, ax1 = plt.subplots(subplot_kw={'projection': 'polar'}) 33 | pcm = ax1.pcolormesh(T, R, E.T, shading='auto', cmap='viridis') 34 | ax1.set_theta_zero_location("N") # 0° at North 35 | ax1.set_theta_direction(-1) # clockwise 36 | ax1.set_title(f"PM Stokes Directional Spectrum\n{os.path.basename(fname)}") 37 | cbar = plt.colorbar(pcm, ax=ax1, orientation="vertical", label="E(f,θ) [m²/Hz/deg]") 38 | 39 | # === 2. 3D Surface Plot === 40 | T_grid, F_grid = np.meshgrid(thetas_deg, freqs) # degrees on axis 41 | fig2 = plt.figure() 42 | ax2 = fig2.add_subplot(111, projection='3d') 43 | surf = ax2.plot_surface(F_grid, T_grid, E, cmap='viridis', linewidth=0, antialiased=True) 44 | ax2.set_xlabel("Frequency [Hz]") 45 | ax2.set_ylabel("Direction [deg]") 46 | ax2.set_zlabel("E(f,θ)") 47 | ax2.set_title(f"PM Stokes Directional Spectrum (3D)\n{os.path.basename(fname)}") 48 | fig2.colorbar(surf, shrink=0.5, aspect=10, label="E(f,θ)") 49 | 50 | plt.show() 51 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/flow_diag.adoc: -------------------------------------------------------------------------------- 1 | [plantuml, format="svg", id="diagram"] 2 | ---- 3 | @startuml 4 | skinparam linetype ortho 5 | rectangle " IMU " as IMU 6 | rectangle " AHRS (Mahony, KalmanQMEKF) " as AHRS 7 | rectangle " Frequency Tracker: \n\n Aranovskiy\n Kalm_ANF\n Zero Crossing" as FreqTracker 8 | rectangle " Kalman Wave Direction " as WaveDir 9 | rectangle " Sea State Detection " as SeaReg 10 | rectangle " Kalman Heave Reconstruction \n\n Kalman Integration with Drift Correction \nKalman Integration with Drift and Motor Noise Filtering" as KalmanWave 11 | rectangle "Process Noise Adaptation" as NoiseAdapt 12 | rectangle " NMEA Data Stream (XDR Sentences) " as NMEA 13 | rectangle " MinMaxLemire " as MinMax 14 | rectangle " De-Spike " as DeSpike 15 | rectangle " Wave Profile Predictor " as WavePredictor 16 | 17 | IMU --> AHRS : " accel " 18 | IMU --> AHRS : " gyro " 19 | IMU --> AHRS : " magn " 20 | AHRS --> FreqTracker : a_{z} 21 | 22 | FreqTracker --> SeaReg : f_{est} 23 | AHRS --> SeaReg : a_{z} 24 | 25 | AHRS --> WaveDir : a_{x}, a_{y} 26 | FreqTracker --> WaveDir : f_{est} 27 | 28 | SeaReg --> NoiseAdapt : R_{reg}, H_{est} 29 | 30 | FreqTracker --> KalmanWave : f_{est} 31 | NoiseAdapt --> KalmanWave : Q 32 | 33 | AHRS --> DeSpike : a_{z} 34 | 35 | DeSpike --> KalmanWave : a_{z} 36 | KalmanWave --> MinMax : " heave " 37 | 38 | KalmanWave --> WavePredictor : " heave " 39 | 40 | WaveDir --> NMEA : " angle, dir " 41 | MinMax --> NMEA : " wave height " 42 | KalmanWave --> NMEA : " heave\n period\n length\n speed " 43 | AHRS --> NMEA : " roll\n pitch\n yaw\n rate of turn " 44 | 45 | @enduml 46 | ---- 47 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/wave_dir_plots.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | import matplotlib as mpl 4 | import numpy as np 5 | 6 | # Configure matplotlib to use LaTeX fonts and export PGF 7 | mpl.use("pgf") 8 | plt.rcParams.update({ 9 | "pgf.texsystem": "xelatex", 10 | "font.family": "serif", 11 | "text.usetex": True, 12 | "pgf.rcfonts": False, 13 | "pgf.preamble": "\n".join([ 14 | r"\usepackage{fontspec}", 15 | r"\usepackage{unicode-math}", 16 | r"\usepackage{amsmath}", 17 | r"\setmainfont{DejaVu Serif}", 18 | r"\setmathfont{Latin Modern Math}", 19 | r"\providecommand{\mathdefault}[1]{#1}" 20 | ]) 21 | }) 22 | 23 | df = pd.read_csv("wave_dir.csv") 24 | 25 | t = df["t"] 26 | ax = df["ax"] 27 | ay = df["ay"] 28 | fax = df["filtered_ax"] 29 | fay = df["filtered_ay"] 30 | freq = df["freq_hz"] 31 | amp = df["amplitude"] 32 | phase = df["phase"] 33 | conf = df["confidence"] 34 | deg = df["deg"] 35 | uncertaintyDeg = df["uncertaintyDeg"] 36 | 37 | # Plot raw and filtered signals 38 | plt.figure(figsize=(10, 13)) 39 | 40 | plt.subplot(5, 1, 1) 41 | #plt.plot(t, ax, label="Raw $a_x$") 42 | plt.plot(t, fax, label="Filtered $a_x$") 43 | #plt.plot(t, ay, label="Raw $a_y$") 44 | plt.plot(t, fay, label="Filtered $a_y$") 45 | plt.plot(t, amp, label="Amplitude") 46 | plt.legend() 47 | plt.title("Raw vs Filtered Signals") 48 | plt.ylabel("Acceleration") 49 | 50 | plt.subplot(5, 1, 2) 51 | plt.plot(t, deg) 52 | plt.title("Estimated Angle") 53 | plt.ylabel("Angle") 54 | 55 | plt.subplot(5, 1, 3) 56 | plt.scatter(ax, ay) 57 | plt.scatter(fax, fay) 58 | plt.title("$A_{est}$") 59 | plt.xlabel("X") 60 | plt.ylabel("Y") 61 | 62 | plt.subplot(5, 1, 4) 63 | plt.plot(t, phase) 64 | plt.title("Estimated Phase") 65 | plt.xlabel("Time, sec") 66 | plt.ylabel("Phase, rad") 67 | 68 | plt.subplot(5, 1, 5) 69 | plt.plot(t, uncertaintyDeg) 70 | plt.title("Uncertainty") 71 | plt.xlabel("Time, sec") 72 | plt.ylabel("Uncertainty, deg") 73 | 74 | plt.tight_layout() 75 | plt.savefig("wave_dir.pgf", bbox_inches='tight') 76 | plt.close() 77 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-state.tex-part: -------------------------------------------------------------------------------- 1 | \section{State Definition} 2 | \label{sec:state} 3 | 4 | The extended state vector of our filter combines 5 | rotational attitude, linear kinematics, and stochastic acceleration subsystems within a 6 | single nonlinear estimator. It is defined as 7 | \begin{equation} 8 | \label{eq:state-vector} 9 | \vct{x} = 10 | \begin{bmatrix} 11 | \delta\vct{\theta} & 12 | \vct{b}_g & 13 | \vct{v} & 14 | \vct{p} & 15 | \vct{S} & 16 | \vct{a}_w & 17 | \vct{b}_a 18 | \end{bmatrix}^{\!\top}, 19 | \end{equation} 20 | where each subvector has the following physical meaning: 21 | \begin{itemize} 22 | \item $\delta\vct{\theta}\in\mathbb{R}^3$ --- small attitude-error 23 | vector used in the multiplicative EKF formulation 24 | (right-multiplicative quaternion correction). 25 | \item $\vct{b}_g\in\mathbb{R}^3$ --- gyroscope bias, modeled as a 26 | random-walk process. 27 | \item $\vct{v}\in\mathbb{R}^3$ --- velocity in the world (NED) frame. 28 | \item $\vct{p}\in\mathbb{R}^3$ --- position or displacement in the world frame. 29 | \item $\vct{S}\in\mathbb{R}^3$ --- integral of displacement, 30 | $\vct{S}(t)=\int_0^t\vct{p}(\tau)\,d\tau$, used to constrain 31 | long-term drift by a zero pseudo-measurement. 32 | \item $\vct{a}_w\in\mathbb{R}^3$ --- latent world-frame acceleration 33 | following an Ornstein--Uhlenbeck (OU) process, 34 | \( 35 | \dot{\vct{a}}_w = -\tfrac{1}{\tau_{a_w}}\vct{a}_w 36 | + \vct{\eta}_{w}(t) 37 | \), 38 | representing correlated ocean-wave forcing. 39 | \item $\vct{b}_a\in\mathbb{R}^3$ --- random-walk accelerometer bias with optional 40 | temperature-dependent drift. 41 | \end{itemize} 42 | 43 | The complete state dimension is 44 | \[ 45 | \fitcol{ 46 | N_X = 47 | \underbrace{3}_{\delta\vct{\theta}} + 48 | \underbrace{3}_{\vct{b}_g} + 49 | \underbrace{3}_{\vct{v}} + 50 | \underbrace{3}_{\vct{p}} + 51 | \underbrace{3}_{\vct{S}} + 52 | \underbrace{3}_{\vct{a}_w} + 53 | \underbrace{3}_{\vct{b}_a} 54 | = 21, 55 | } 56 | \] 57 | with both gyro and accelerometer bias. 58 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_results.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | Header = np.loadtxt(fname="results.csv", delimiter=",", 5 | usecols=(1, 3, 5, 7), 6 | skiprows=0, max_rows=1) 7 | 8 | main_amp = Header[0] 9 | main_freq = Header[1] 10 | acc_bias = Header[2] 11 | acc_noise_std_dev = Header[3] 12 | 13 | Data = np.loadtxt(fname="results.csv", delimiter=",", 14 | usecols=(1, 3, 5, 7, 9, 11, 13, 15, 17, 19, 21, 23, 25, 27, 29, 31, 33, 35), 15 | skiprows=1) 16 | 17 | Time = Data[:, [0]] 18 | AccX = Data[:, [1]] 19 | RefVelX = Data[:, [2]] 20 | RefPosX = Data[:, [3]] 21 | Heave = Data[:, [4]] 22 | HeaveAlt = Data[:, [5]] 23 | Freq = Data[:, [10]] 24 | FreqAdj = Data[:, [11]] 25 | RefFreq = Data[:, [14]] 26 | HeaveAltErr = Data[:, [15]] 27 | FreqAdjErr = Data[:, [16]] 28 | HeavePredict = Data[:, [17]] 29 | 30 | rms = np.sqrt(np.mean(HeaveAltErr[-480 * 60:] ** 2)) 31 | rms_freq = np.sqrt(np.mean(FreqAdjErr[-480 * 60:] ** 2)) 32 | 33 | fig, axarr = plt.subplots(4, sharex="all", figsize=(12, 9)) 34 | 35 | axarr[0].set_title('Acceleration (m/s^2) bias=' + str(acc_bias) + ' noise_std_dev= ' + str(acc_noise_std_dev)) 36 | axarr[0].plot(Time, AccX, label="Input Acc") 37 | axarr[0].grid() 38 | axarr[0].legend() 39 | 40 | axarr[1].set_title('Freq (Hz) main_freq=' + str(main_freq) + " err_rms=" + str(rms_freq)) 41 | axarr[1].plot(Time, Freq, "r-", label="Freq") 42 | axarr[1].plot(Time, FreqAdj, "g-", label="FreqAdj") 43 | axarr[1].plot(Time, RefFreq, "b-", label="RefFreq") 44 | axarr[1].grid() 45 | axarr[1].legend() 46 | 47 | axarr[2].set_title('Heave (m) main_amplitude=' + str(main_amp)) 48 | axarr[2].plot(Time, RefPosX, label="Reference Pos") 49 | axarr[2].plot(Time, Heave, "r-", label="Heave") 50 | axarr[2].plot(Time, HeaveAlt, "g-", label="HeaveAlt") 51 | #axarr[2].plot(Time, HeavePredict, "-", label="HeavePredict") 52 | axarr[2].grid() 53 | axarr[2].legend() 54 | 55 | axarr[3].set_title('Heave Err (m) rms=' + str(rms)) 56 | axarr[3].plot(Time, HeaveAltErr, "r-", label="HeaveAltErr") 57 | axarr[3].grid() 58 | axarr[3].legend() 59 | 60 | fig.savefig("results.svg", dpi=600) 61 | 62 | plt.show() 63 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-lti-discrete.tex-part: -------------------------------------------------------------------------------- 1 | \subsection{Discretization of a generic LTI SDE over step \texorpdfstring{$h$}{h}} 2 | \label{sec:lti-disc} 3 | 4 | Consider a linear time–invariant SDE 5 | \begin{equation} 6 | \fitcol{ 7 | \dot{\delta\vct{x}}(t) 8 | = \mat{F}\,\delta\vct{x}(t) + \mat{L}\,\vct{w}_c(t), 9 | \qquad 10 | \mathbb{E}\bigl[\vct{w}_c(t)\vct{w}_c^\top(s)\bigr] 11 | = \matg{Q}_c\,\delta(t{-}s), 12 | } 13 | \end{equation} 14 | with Jacobian (linearized) dynamics matrix $\mat{F}$, noise–input matrix $\mat{L}$, 15 | and continuous spectral density $\matg{Q}_c$. 16 | 17 | \paragraph{State transition via the matrix exponential} 18 | Over a small step $h>0$, the exact discrete transition of the mean is 19 | \begin{equation} 20 | \delta\vct{x}(t{+}h) 21 | \;=\; 22 | \underbrace{e^{\mat{F} h}}_{\displaystyle \matg{\Phi}(h)}\,\delta\vct{x}(t) 23 | \;+\; 24 | \int_{0}^{h} e^{\mat{F} (h-\tau)}\,\mat{L}\,\vct{w}_c(t{+}\tau)\,d\tau. 25 | \end{equation} 26 | The matrix exponential is defined by the absolutely convergent series 27 | $e^{\mat{F} h}=\sum_{k=0}^{\infty}\frac{(\mat{F} h)^k}{k!}$. 28 | In this setting $\matg{\Phi}(h)=e^{\mat{F} h}$ is the 29 | \emph{discrete state transition}. 30 | 31 | \paragraph{Discrete process covariance} 32 | Let $\matg{Q}_d(h)$ denote the process noise accrued over $[t,t{+}h]$. 33 | From the stochastic variation-of-constants formula, 34 | \begin{equation} 35 | \matg{Q}_d(h) 36 | \;=\; 37 | \int_{0}^{h} 38 | e^{\mat{F} \tau}\,\mat{L}\,\matg{Q}_c\,\mat{L}^\top\, 39 | e^{\mat{F}^\top \tau}\,d\tau. 40 | \label{eq:Qd-integral} 41 | \end{equation} 42 | 43 | \paragraph{Discrete Kalman prediction (time update)} 44 | Given $(\hat{\vct{x}}_k,\mat{P}_k)$ at $t_k$, the prediction to $t_{k+1}=t_k{+}h$ is 45 | \begin{align} 46 | \hat{\vct{x}}_{k+1|k} &= \matg{\Phi}(h)\,\hat{\vct{x}}_k,\\ 47 | \mat{P}_{k+1|k} &= \matg{\Phi}(h)\,\mat{P}_k\,\matg{\Phi}(h)^\top \;+\; \matg{Q}_d(h). 48 | \end{align} 49 | These equations are the discrete counterparts of the continuous Lyapunov/Riccati 50 | flow with $\mat{F}$ and $\mat{L}$ the Jacobian dynamics and noise–input matrices 51 | defined in \S\ref{sec:lti-sde}. In our implementation, $\matg{\Phi}$ and $\matg{Q}_d$ 52 | are computed analytically for the OU–driven kinematic blocks. 53 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_fenton_wave_tracker.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import pandas as pd 4 | 5 | def visualize_wave_data(csv_file='wave_tracker_data.csv'): 6 | # Read the data from CSV file 7 | try: 8 | data = pd.read_csv(csv_file) 9 | except FileNotFoundError: 10 | print(f"Error: File '{csv_file}' not found. Please run the FentonWave_test first.") 11 | return 12 | 13 | # Create a figure with 3 subplots 14 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(10, 12), sharex=True) 15 | 16 | # Plot displacement 17 | ax1.plot(data['Time(s)'][1:], data['Displacement(m)'][1:], 'b-', label='Surface Elevation') 18 | ax1.set_ylabel('Displacement (m)') 19 | ax1.set_title('Wave Surface Kinematics') 20 | ax1.grid(True) 21 | ax1.legend() 22 | 23 | # Plot velocity 24 | ax2.plot(data['Time(s)'][1:], data['Velocity(m/s)'][1:], 'r-', label='Vertical Velocity') 25 | ax2.set_ylabel('Velocity (m/s)') 26 | ax2.grid(True) 27 | ax2.legend() 28 | 29 | # Plot acceleration 30 | ax3.plot(data['Time(s)'], data['Acceleration(m/s²)'], 'g-', label='Vertical Acceleration') 31 | ax3.set_xlabel('Time (s)') 32 | ax3.set_ylabel('Acceleration (m/s²)') 33 | ax3.grid(True) 34 | ax3.legend() 35 | 36 | # Add some statistics to the plots 37 | stats_text = ( 38 | f"Max displacement: {data['Displacement(m)'].max():.2f} m\n" 39 | f"Max velocity: {data['Velocity(m/s)'].max():.2f} m/s\n" 40 | f"Max acceleration: {data['Acceleration(m/s²)'].max():.2f} m/s²" 41 | ) 42 | ax1.text(0.02, 0.98, stats_text, transform=ax1.transAxes, 43 | verticalalignment='top', bbox=dict(boxstyle='round', alpha=0.5)) 44 | 45 | plt.tight_layout() 46 | plt.show() 47 | 48 | # Optional: Plot the wave profile at a specific time 49 | plot_wave_profile(data) 50 | 51 | def plot_wave_profile(data): 52 | # For the wave profile, we'll use the X_Position data 53 | # Since the particle is following the crest, we can plot displacement vs position 54 | plt.figure(figsize=(10, 5)) 55 | plt.plot(data['X_Position(m)'], data['Displacement(m)'], 'b-') 56 | plt.xlabel('Horizontal Position (m)') 57 | plt.ylabel('Surface Elevation (m)') 58 | plt.title('Wave Profile Along Particle Path') 59 | plt.grid(True) 60 | plt.show() 61 | 62 | if __name__ == "__main__": 63 | visualize_wave_data() 64 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-cross-cov.tex-part: -------------------------------------------------------------------------------- 1 | \section{Kronecker Structure and Cross-Covariance Propagation} 2 | 3 | \subsection{Kronecker Construction of Translational Noise} 4 | 5 | The OU-driven translational chain is separable by spatial axis. 6 | Let $\mathbf{Q}^{(1)}_d$ denote the $4\times4$ discrete covariance for one axis 7 | as derived in Section~\ref{sec:ou-primitives}. 8 | For a correlated three-axis driving process with stationary covariance 9 | $\Sigma_{a_w}$, the full $12\times12$ translational block is constructed as 10 | \[ 11 | \mathbf{Q}_{LL} 12 | = (\Sigma_{a_w}^{1/2}\!\otimes I_4)\, 13 | (I_3\!\otimes \mathbf{Q}^{(1)}_d)\, 14 | (\Sigma_{a_w}^{1/2}\!\otimes I_4)^\top. 15 | \] 16 | This Kronecker structure compactly expresses the correlation of OU forcing 17 | across $x$, $y$, and $z$ directions while preserving the per-axis temporal 18 | integration dynamics. In implementation, this is realized by scaling each 19 | $4\times4$ axis block of $\mathbf{Q}^{(1)}_d$ by the corresponding entry 20 | $\Sigma_{a_w}(i,j)$ and explicitly symmetrizing the result. 21 | 22 | \subsection{Cross-Covariance Evolution Between Subsystems} 23 | 24 | Let the covariance be partitioned as 25 | \[ 26 | P = 27 | \begin{bmatrix} 28 | P_{AA} & P_{AL} & P_{A b_a}\\[2pt] 29 | P_{AL}^\top & P_{LL} & P_{L b_a}\\[2pt] 30 | P_{A b_a}^\top & P_{L b_a}^\top & P_{b_a b_a} 31 | \end{bmatrix}. 32 | \] 33 | During prediction, the cross terms evolve as 34 | \begin{align} 35 | P_{AL}^{+} &= \Phi_{AA}P_{AL}\Phi_{LL}^\top,\\ 36 | P_{A b_a}^{+} &= \Phi_{AA}P_{A b_a},\\ 37 | P_{L b_a}^{+} &= \Phi_{LL}P_{L b_a}, 38 | \end{align} 39 | and their transposes are restored after symmetrization. 40 | These blocks transmit rotational uncertainty into linear motion and bias 41 | subspaces, ensuring physically correct coupling between orientation drift, 42 | velocity error, and bias-induced acceleration misalignment. 43 | 44 | \subsection{Joseph-Form Update Consistency} 45 | 46 | During each measurement update, the full covariance---including 47 | cross-blocks---is corrected via the Joseph form, 48 | \[ 49 | P^{+} = P^{-} - K C P^{-} - (K C P^{-})^\top + K R K^\top, 50 | \] 51 | so that correlations between attitude and linear states are adjusted 52 | consistently with the innovation residual. This preserves observability 53 | relations: accelerometer updates affect not only attitude but also 54 | velocity and displacement uncertainty through $P_{AL}$. 55 | 56 | \subsection{Numerical Enforcement} 57 | 58 | After propagation and update, 59 | \[ 60 | P \leftarrow \tfrac12(P+P^\top), 61 | \] 62 | and the translational block is projected to the nearest positive-semidefinite 63 | matrix. These steps prevent loss of symmetry due to finite precision and 64 | ensure that all cross-covariances remain consistent with the Kronecker-based 65 | OU noise model. 66 | 67 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/spectr_sim_plots.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import matplotlib.pyplot as plt 3 | import glob 4 | import os 5 | 6 | def load_spectrum_csv(filename): 7 | return pd.read_csv(filename) 8 | 9 | def plot_last_block(files): 10 | for fname in files: 11 | df = pd.read_csv(fname) 12 | 13 | # spectrum columns 14 | est_cols = [c for c in df.columns if c.startswith("S_eta_est_f")] 15 | ref_cols = [c for c in df.columns if c.startswith("S_eta_ref_f")] 16 | 17 | if not est_cols or not ref_cols: 18 | print(f"[warn] no spectrum columns in {fname}") 19 | continue 20 | 21 | # drop incomplete rows, then pick the row with the largest block 22 | df_clean = df.dropna(subset=est_cols) 23 | if df_clean.empty: 24 | print(f"[warn] no complete spectra in {fname}") 25 | continue 26 | 27 | if "block" in df_clean.columns: 28 | last_row = df_clean.loc[df_clean["block"].idxmax()] 29 | block_id = int(last_row["block"]) 30 | else: 31 | last_row = df_clean.iloc[-1] 32 | block_id = len(df_clean) - 1 33 | 34 | # parse freqs from headers 35 | freqs = [] 36 | for c in est_cols: 37 | if "Hz=" in c: 38 | hz = float(c.split("Hz=")[1]) 39 | freqs.append(hz) 40 | else: 41 | freqs.append(len(freqs)) 42 | 43 | est = last_row[est_cols].to_numpy(dtype=float) 44 | ref = last_row[ref_cols].to_numpy(dtype=float) 45 | Hs = float(last_row["Hs"]) 46 | Fp = float(last_row["Fp"]) 47 | 48 | plt.figure() 49 | plt.loglog(freqs, est, "o-", label="Estimated (last block)") 50 | plt.loglog(freqs, ref, "x--", label="Reference") 51 | plt.xlabel("Frequency [Hz]") 52 | plt.ylabel("S_eta [m^2/Hz]") 53 | plt.title(f"{os.path.basename(fname)} — Block {block_id} (Hs={Hs:.2f} m, Fp={Fp:.3f} Hz)") 54 | plt.grid(True, which="both") 55 | plt.legend() 56 | 57 | plt.show() 58 | 59 | def discover_files(): 60 | files = sorted(glob.glob("spectrum_*.csv")) 61 | # keep only JONSWAP or PM files 62 | return [f for f in files if ("JONSWAP" in f.upper() or "PM" in f.upper())] 63 | 64 | if __name__ == "__main__": 65 | import argparse 66 | parser = argparse.ArgumentParser(description="Plot last-block spectrum CSVs (only JONSWAP/PM).") 67 | parser.add_argument("--file", type=str, default=None, 68 | help="Single spectrum CSV file") 69 | args = parser.parse_args() 70 | 71 | if args.file: 72 | files = [args.file] 73 | else: 74 | files = discover_files() 75 | 76 | if not files: 77 | print("No JONSWAP/PM spectrum_*.csv files found.") 78 | else: 79 | print("Plotting last block for files:", files) 80 | plot_last_block(files) 81 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/KalmanSmoother.h: -------------------------------------------------------------------------------- 1 | #ifndef KalmanSmoother_h 2 | #define KalmanSmoother_h 3 | 4 | /* 5 | A one dimensional Kalman filter implementation - a single variable smoother filter 6 | 7 | The variables are: 8 | 9 | x for the filtered value, 10 | q for the process noise, 11 | r for the measurement (sensor) uncertainty, 12 | p for the estimation uncertainty, 13 | k for the Kalman gain. 14 | 15 | The state of the filter is defined by the values of these variables. 16 | 17 | The initial values for p is not very important since it is adjusted 18 | during the process. It must be just high enough to narrow down. 19 | 20 | q - usually a small number between 0.001 and 1 - how fast your measurement moves. 21 | Recommended 0.01. Should be tunned to your needs. 22 | 23 | But tweaking the values for the process noise and sensor noise 24 | is essential to get clear readouts. 25 | 26 | For large noise reduction, you can try to start from: 27 | (see http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/ ) 28 | 29 | q = 0.002 30 | r = 30.0 31 | p = 2000.0 // "large enough to narrow down" 32 | 33 | Example: 34 | 35 | KalmanSmootherVars kalman; 36 | kalman_smoother_init(&kalman, 0.002, 30.0, 2000.0); 37 | kalman_smoother_set_initial(&kalman, 1.0); 38 | while(1) { 39 | double measure = getSensorValue(); 40 | double measure_adjusted = kalman_smoother_update(&kalman, measure); 41 | } 42 | 43 | Used also with results of Aranovskiy filter. So use double instead of float to 44 | avoid decimal overflows with higher Aranovskiy gain values. 45 | 46 | */ 47 | 48 | typedef struct kalman_smoother_vars { 49 | /* Kalman filter variables */ 50 | double q; // process noise covariance 51 | double r; // measurement uncertainty 52 | double p; // estimation uncertainty 53 | double k; // kalman gain 54 | double x; // value 55 | } KalmanSmootherVars; 56 | 57 | void kalman_smoother_init(KalmanSmootherVars* s, double process_noise, double sensor_noise, double estimated_error); 58 | void kalman_smoother_set_initial(KalmanSmootherVars* s, double intial_value); 59 | double kalman_smoother_update(KalmanSmootherVars* s, double measurement); 60 | 61 | void kalman_smoother_init(KalmanSmootherVars* s, double process_noise, double sensor_noise, double estimated_error) { 62 | s->q = process_noise; 63 | s->r = sensor_noise; 64 | s->p = estimated_error; 65 | } 66 | 67 | void kalman_smoother_set_initial(KalmanSmootherVars* s, double initial_value) { 68 | s->x = initial_value; 69 | } 70 | 71 | double kalman_smoother_update(KalmanSmootherVars* s, double measurement) { 72 | // measurement update 73 | s->k = s->p / (s->p + s->r); 74 | double current_estimate = s->x + s->k * (measurement - s->x); 75 | s->p = (1.0 - s->k) * s->p + fabs(s->x - current_estimate) * s->q; 76 | s->x = current_estimate; 77 | return s->x; 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_freqs.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | import glob 4 | import pandas as pd 5 | import matplotlib.pyplot as plt 6 | 7 | # Adjust this to your folder with CSV files 8 | DATA_FOLDER = "./" 9 | 10 | # Updated regex to include pmstokes (PN) waves 11 | FNAME_RE = re.compile( 12 | r'^(aranovskiy|kalmANF|zerocrossing)_(gerstner|jonswap|fenton|pmstokes)_h([0-9.]+)\.csv$' 13 | ) 14 | 15 | # Load all CSV files 16 | def load_all_data(folder): 17 | data = [] 18 | for filepath in glob.glob(os.path.join(folder, "*.csv")): 19 | fname = os.path.basename(filepath) 20 | m = FNAME_RE.match(fname) 21 | if not m: 22 | continue 23 | tracker, wave, height_str = m.groups() 24 | height = float(height_str) 25 | df = pd.read_csv(filepath) 26 | df['tracker'] = tracker 27 | df['wave'] = wave 28 | df['height'] = height 29 | data.append(df) 30 | if not data: 31 | raise RuntimeError("No matching CSV files found in folder") 32 | return pd.concat(data, ignore_index=True) 33 | 34 | # Plot for each (wave, height) scenario all trackers on same figure 35 | def plot_scenarios(df): 36 | scenarios = df.groupby(['wave', 'height']) 37 | for (wave, height), group in scenarios: 38 | plt.figure(figsize=(12, 6)) 39 | plt.title(f"Frequency Tracking Comparison\nWave: {wave} Height: {height:.3f} m") 40 | plt.xlabel("Time (s)") 41 | plt.ylabel("Frequency (Hz)") 42 | 43 | for tracker in group['tracker'].unique(): 44 | subset = group[group['tracker'] == tracker] 45 | 46 | # Plot raw estimate freq 47 | plt.plot(subset['time'], subset['est_freq'], label=f"{tracker} raw") 48 | 49 | # Plot smoothed freq 50 | plt.plot(subset['time'], subset['smooth_freq'], linestyle='--', label=f"{tracker} smooth") 51 | 52 | plt.legend() 53 | plt.grid(True) 54 | plt.tight_layout() 55 | plt.show() 56 | 57 | # Optional: plot error for a scenario (wave, height) 58 | def plot_errors(df, wave, height): 59 | subset = df[(df['wave'] == wave) & (df['height'] == height)] 60 | plt.figure(figsize=(12,6)) 61 | plt.title(f"Frequency Tracking Errors\nWave: {wave} Height: {height:.3f} m") 62 | plt.xlabel("Time (s)") 63 | plt.ylabel("Frequency Error (Hz)") 64 | 65 | for tracker in subset['tracker'].unique(): 66 | tr_data = subset[subset['tracker'] == tracker] 67 | plt.plot(tr_data['time'], tr_data['error'], label=f"{tracker} raw error") 68 | plt.plot(tr_data['time'], tr_data['smooth_error'], linestyle='--', label=f"{tracker} smooth error") 69 | 70 | plt.legend() 71 | plt.grid(True) 72 | plt.tight_layout() 73 | plt.show() 74 | 75 | def main(): 76 | df = load_all_data(DATA_FOLDER) 77 | print(f"Loaded {len(df)} rows from CSV files.") 78 | plot_scenarios(df) 79 | 80 | # Uncomment to plot error for a specific wave/height 81 | # plot_errors(df, 'pmstokes', 0.75) 82 | 83 | if __name__ == "__main__": 84 | main() 85 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/fenton_plots.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | import matplotlib as mpl 4 | import numpy as np 5 | 6 | # Configure matplotlib to use LaTeX fonts and export PGF 7 | mpl.use("pgf") 8 | plt.rcParams.update({ 9 | "pgf.texsystem": "xelatex", 10 | "font.family": "serif", 11 | "text.usetex": True, 12 | "pgf.rcfonts": False, 13 | "pgf.preamble": "\n".join([ 14 | r"\usepackage{fontspec}", 15 | r"\usepackage{unicode-math}", 16 | r"\usepackage{amsmath}", 17 | r"\setmainfont{DejaVu Serif}", 18 | r"\setmathfont{Latin Modern Math}", 19 | r"\providecommand{\mathdefault}[1]{#1}" 20 | ]) 21 | }) 22 | 23 | def plot_fenton_wave(): 24 | data = pd.read_csv("wave_data.csv") 25 | 26 | plt.figure(figsize=(10, 4)) 27 | plt.plot(data['x'], data['elevation'], label=r'Surface Elevation $\eta(x)$', color='blue') 28 | plt.xlabel("Horizontal Position $x$ (m)") 29 | plt.ylabel(r'Surface Elevation $\eta$ (m)') 30 | plt.title("Fenton Wave Profile") 31 | plt.grid(True) 32 | plt.legend() 33 | plt.tight_layout() 34 | plt.savefig("wave_profile.pgf", bbox_inches='tight') 35 | plt.close() 36 | 37 | def plot_wave_kinematics(): 38 | try: 39 | data = pd.read_csv("wave_tracker_data.csv") 40 | except FileNotFoundError: 41 | print("File 'wave_tracker_data.csv' not found.") 42 | return 43 | 44 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(10, 12), sharex=True) 45 | 46 | ax1.plot(data['Time(s)'][1:], data['Displacement(m)'][1:], 'b-', label='Surface Elevation') 47 | ax1.set_ylabel('Displacement (m)') 48 | ax1.set_title('Surface Float Kinematics') 49 | ax1.grid(True) 50 | ax1.legend() 51 | 52 | ax2.plot(data['Time(s)'][1:], data['Velocity(m/s)'][1:], 'r-', label='Vertical Velocity') 53 | ax2.set_ylabel('Velocity (m/s)') 54 | ax2.grid(True) 55 | ax2.legend() 56 | 57 | ax3.plot(data['Time(s)'], data['Acceleration(m/s²)'], 'g-', label='Vertical Acceleration') 58 | ax3.set_xlabel('Time (s)') 59 | ax3.set_ylabel('Acceleration (m/s²)') 60 | ax3.grid(True) 61 | ax3.legend() 62 | 63 | stats_text = ( 64 | f"Max displacement: {data['Displacement(m)'].max():.2f} m\n" 65 | f"Max velocity: {data['Velocity(m/s)'].max():.2f} m/s\n" 66 | f"Max acceleration: {data['Acceleration(m/s²)'].max():.2f} m/s²" 67 | ) 68 | ax1.text(0.02, 0.98, stats_text, transform=ax1.transAxes, 69 | verticalalignment='top', bbox=dict(boxstyle='round', alpha=0.5)) 70 | 71 | plt.tight_layout() 72 | plt.savefig("wave_kinematics.pgf", bbox_inches='tight') 73 | plt.close() 74 | 75 | def plot_wave_profile_path(): 76 | try: 77 | data = pd.read_csv("wave_tracker_data.csv") 78 | except FileNotFoundError: 79 | print("File 'wave_tracker_data.csv' not found.") 80 | return 81 | 82 | plt.figure(figsize=(10, 5)) 83 | plt.plot(data['X_Position(m)'], data['Displacement(m)'], 'b-') 84 | plt.xlabel('Horizontal Position (m)') 85 | plt.ylabel('Surface Elevation (m)') 86 | plt.title('Wave Profile Along Particle Path') 87 | plt.grid(True) 88 | plt.tight_layout() 89 | plt.savefig("wave_particle_path.pgf", bbox_inches='tight') 90 | plt.close() 91 | 92 | if __name__ == "__main__": 93 | plot_fenton_wave() 94 | plot_wave_kinematics() 95 | plot_wave_profile_path() 96 | print("All PGF plots saved successfully.") 97 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/fusion_diag_plots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import glob 3 | import os 4 | import re 5 | import pandas as pd 6 | import matplotlib as mpl 7 | import matplotlib.pyplot as plt 8 | 9 | # === PGF / LaTeX friendly output (optional) === 10 | mpl.use("pgf") 11 | plt.rcParams.update({ 12 | "pgf.texsystem": "xelatex", 13 | "font.family": "serif", 14 | "text.usetex": True, 15 | "pgf.rcfonts": False, 16 | "pgf.preamble": "\n".join([ 17 | r"\usepackage{fontspec}", 18 | r"\usepackage{unicode-math}", 19 | r"\usepackage{amsmath}", 20 | r"\setmainfont{DejaVu Serif}", 21 | r"\setmathfont{Latin Modern Math}", 22 | r"\providecommand{\mathdefault}[1]{#1}" 23 | ]) 24 | }) 25 | 26 | # === Configuration === 27 | DATA_DIR = "./" 28 | SAMPLE_RATE_HZ = 240 29 | SKIP_TIME_S = 1140.0 # skip initial transient 30 | PLOT_TIME_S = 60.0 # plot next 60 seconds 31 | MAX_TIME_S = SKIP_TIME_S + PLOT_TIME_S 32 | MAX_ROWS = int(SAMPLE_RATE_HZ * MAX_TIME_S) 33 | 34 | # === Regex to find valid files === 35 | pattern = re.compile(r".*_fusion.*\.csv$", re.IGNORECASE) 36 | 37 | # === Utility === 38 | def latex_safe(s): 39 | return re.sub(r"([_#%$&{}])", r"\\\1", s) 40 | 41 | def make_subplots(nrows, title, width=8, row_height=2.0): 42 | fig_height = row_height * nrows 43 | fig, axes = plt.subplots(nrows, 1, figsize=(width, fig_height), sharex=True) 44 | fig.suptitle(title, fontsize=11) 45 | if nrows == 1: 46 | axes = [axes] 47 | return fig, axes 48 | 49 | def finalize_plot(fig, outbase, suffix=""): 50 | fig.tight_layout(rect=[0, 0.03, 1, 0.95]) 51 | for ext in ("pgf", "svg", "png"): 52 | fig.savefig(f"{outbase}{suffix}.{ext}", bbox_inches="tight") 53 | plt.close(fig) 54 | 55 | # === Main === 56 | files = [f for f in glob.glob(os.path.join(DATA_DIR, "*_fusion*.csv")) if pattern.match(f)] 57 | if not files: 58 | print("No *_fusion*.csv files found.") 59 | exit() 60 | 61 | for fname in sorted(files): 62 | base = os.path.basename(fname) 63 | print(f"Plotting {base} ...") 64 | df = pd.read_csv(fname, nrows=MAX_ROWS) 65 | if "time" not in df.columns: 66 | print(f"Skipping {fname}: no 'time' column.") 67 | continue 68 | 69 | # Subset time range 70 | df = df[(df["time"] >= SKIP_TIME_S) & (df["time"] <= MAX_TIME_S)].reset_index(drop=True) 71 | if df.empty: 72 | print(f"Skipping {fname}: no data after {SKIP_TIME_S}s.") 73 | continue 74 | time = df["time"] 75 | 76 | # Required columns (applied values) 77 | required = ["freq_tracker_hz", "sigma_a_applied", "tau_applied", "R_S_applied"] 78 | missing = [c for c in required if c not in df.columns] 79 | if missing: 80 | print(f"Missing columns in {fname}: {missing}") 81 | continue 82 | 83 | # === Plot === 84 | fig, axes = make_subplots(4, latex_safe(base) + " (Adaptive Parameters)") 85 | 86 | axes[0].plot(time, df["freq_tracker_hz"], color="tab:blue", linewidth=1.2) 87 | axes[0].set_ylabel(r"$f_\mathrm{tracker}$ [Hz]") 88 | axes[0].grid(True) 89 | 90 | axes[1].plot(time, df["sigma_a_applied"], color="tab:green", linewidth=1.2) 91 | axes[1].set_ylabel(r"$\sigma_a$ [m/s$^2$]") 92 | axes[1].grid(True) 93 | 94 | axes[2].plot(time, df["tau_applied"], color="tab:orange", linewidth=1.2) 95 | axes[2].set_ylabel(r"$\tau$ [s]") 96 | axes[2].grid(True) 97 | 98 | axes[3].plot(time, df["R_S_applied"], color="tab:red", linewidth=1.2) 99 | axes[3].set_ylabel(r"$R_S$") 100 | axes[3].set_xlabel("Time [s]") 101 | axes[3].grid(True) 102 | 103 | outbase = os.path.splitext(fname)[0] + "_params" 104 | finalize_plot(fig, outbase) 105 | 106 | print("Done.") 107 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/FrequencySmoother.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | template 6 | class FrequencySmoother { 7 | public: 8 | /** 9 | * Constructor with tuned parameters for ocean waves 10 | * @param q_short Process noise for short periods (fast waves) 11 | * @param q_long Process noise for long periods (slow waves) 12 | * @param T_min Minimum wave period in seconds 13 | * @param T_max Maximum wave period in seconds 14 | * @param measurement_noise Measurement noise (period jitter) 15 | * @param estimated_error Initial estimation uncertainty 16 | * @param rel_threshold Relative deviation threshold for outlier detection 17 | * @param noise_scale Factor to inflate R for suspicious samples 18 | */ 19 | FrequencySmoother(Real q_short_ = Real(0.00001), 20 | Real q_long_ = Real(0.00010), 21 | Real T_min_ = Real(0.33), 22 | Real T_max_ = Real(15.0), 23 | Real measurement_noise_ = Real(0.05), 24 | Real estimated_error_ = Real(10.0), 25 | Real rel_threshold_ = Real(0.3), 26 | Real noise_scale_ = Real(15.0)) 27 | : q_short(q_short_), q_long(q_long_), 28 | T_min(T_min_), T_max(T_max_), 29 | r(measurement_noise_), 30 | p(estimated_error_), 31 | T(Real(0)), 32 | rel_thresh(rel_threshold_), 33 | r_scale(noise_scale_) 34 | {} 35 | 36 | /// Set initial frequency in Hz 37 | void setInitial(Real freq_hz) { 38 | if (freq_hz > Real(0)) { 39 | T = Real(1) / freq_hz; 40 | } else { 41 | T = Real(0); 42 | } 43 | } 44 | 45 | /// Update with new frequency measurement (Hz) and return smoothed frequency (Hz) 46 | Real update(Real freq_measured_hz) { 47 | if (freq_measured_hz <= Real(0)) { 48 | return (T > Real(0)) ? (Real(1) / T) : Real(0); 49 | } 50 | 51 | Real T_meas = Real(1) / freq_measured_hz; 52 | 53 | if (T <= Real(0)) { 54 | T = T_meas; 55 | return freq_measured_hz; // no smoothing on first sample 56 | } 57 | 58 | // Interpolate process noise q based on current period estimate T 59 | Real q = interpolateProcessNoise(T); 60 | 61 | // Determine if suspicious measurement: relative difference in period 62 | Real r_eff = r; 63 | if (T > Real(0)) { 64 | Real rel_diff = std::fabs(T_meas - T) / T; 65 | if (rel_diff > rel_thresh) { 66 | r_eff *= r_scale; // inflate measurement noise for outlier 67 | } 68 | } 69 | 70 | // Kalman update 71 | Real k_gain = p / (p + r_eff); 72 | Real T_est = T + k_gain * (T_meas - T); 73 | p = (Real(1) - k_gain) * p + std::fabs(T - T_est) * q; 74 | T = T_est; 75 | 76 | return Real(1) / T; // return frequency 77 | } 78 | 79 | private: 80 | Real q_short; 81 | Real q_long; 82 | Real T_min; 83 | Real T_max; 84 | 85 | Real r; // measurement noise 86 | Real p; // estimation error covariance 87 | Real T; // current period estimate 88 | 89 | Real rel_thresh; // relative outlier threshold 90 | Real r_scale; // outlier noise scaling factor 91 | 92 | Real interpolateProcessNoise(Real period) const { 93 | if (period <= T_min) return q_short; 94 | if (period >= T_max) return q_long; 95 | // Linear interpolation 96 | Real alpha = (period - T_min) / (T_max - T_min); 97 | return q_short + alpha * (q_long - q_short); 98 | } 99 | }; 100 | -------------------------------------------------------------------------------- /bbn_imu_stat_m5atomS3/bbn_imu_stat_m5atomS3.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Measure IMU standard deviations 3 | 4 | Copyright 2024, Mikhail Grushinskiy 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "Statistic.h" 11 | 12 | statistic::Statistic accelX; 13 | statistic::Statistic accelY; 14 | statistic::Statistic accelZ; 15 | statistic::Statistic gyroX; 16 | statistic::Statistic gyroY; 17 | statistic::Statistic gyroZ; 18 | 19 | const char* imu_name; 20 | 21 | unsigned long last_update = 0UL, now = 0UL; 22 | 23 | #define SAMPLES_COUNT 2500 24 | 25 | int update_stat(statistic::Statistic *stats, float value, const char* name) { 26 | stats->add(value); 27 | if (stats->count() == SAMPLES_COUNT) { 28 | now = micros(); 29 | Serial.println(name); 30 | Serial.print(" Count: "); 31 | Serial.println(stats->count()); 32 | Serial.print(" Sample Rate: "); 33 | Serial.println(stats->count() / ((now - last_update) / 1000000.0)); 34 | Serial.print(" Min: "); 35 | Serial.println(stats->minimum(), 7); 36 | Serial.print(" Max: "); 37 | Serial.println(stats->maximum(), 7); 38 | Serial.print(" Average: "); 39 | Serial.println(stats->average(), 7); 40 | Serial.print(" variance: "); 41 | Serial.println(stats->variance(), 7); 42 | Serial.print(" pop stdev: "); 43 | Serial.println(stats->pop_stdev(), 7); 44 | Serial.print(" unbias stdev: "); 45 | Serial.println(stats->unbiased_stdev(), 7); 46 | Serial.print(" time(micros): "); 47 | Serial.printf("%d\n", now); 48 | Serial.println("====================================="); 49 | stats->clear(); 50 | } 51 | return stats->count(); 52 | } 53 | 54 | void read_and_processIMU_data() { 55 | m5::imu_3d_t accel; 56 | M5.Imu.getAccelData(&accel.x, &accel.y, &accel.z); 57 | 58 | m5::imu_3d_t gyro; 59 | M5.Imu.getGyroData(&gyro.x, &gyro.y, &gyro.z); 60 | 61 | int count = 62 | update_stat(&gyroX, gyro.x, "gyroX"); 63 | update_stat(&gyroY, gyro.y, "gyroY"); 64 | update_stat(&gyroZ, gyro.z, "gyroZ"); 65 | 66 | update_stat(&accelX, accel.x, "accelX"); 67 | update_stat(&accelY, accel.y, "accelY"); 68 | update_stat(&accelZ, accel.z, "accelZ"); 69 | 70 | if (count == 0) { 71 | last_update = micros(); 72 | } 73 | } 74 | 75 | void repeatMe() { 76 | static uint32_t prev_sec = 0; 77 | auto imu_update = M5.Imu.update(); 78 | if (imu_update) { 79 | read_and_processIMU_data(); 80 | } 81 | int32_t sec = millis() / 1000; 82 | if (prev_sec != sec) { 83 | prev_sec = sec; 84 | if ((sec & 7) == 0) { 85 | // prevent WDT. 86 | vTaskDelay(1); 87 | } 88 | } 89 | } 90 | 91 | void setup() { 92 | auto cfg = M5.config(); 93 | M5.begin(cfg); 94 | Serial.begin(115200); 95 | 96 | auto imu_type = M5.Imu.getType(); 97 | switch (imu_type) { 98 | case m5::imu_none: imu_name = "not found"; break; 99 | case m5::imu_sh200q: imu_name = "sh200q"; break; 100 | case m5::imu_mpu6050: imu_name = "mpu6050"; break; 101 | case m5::imu_mpu6886: imu_name = "mpu6886"; break; 102 | case m5::imu_mpu9250: imu_name = "mpu9250"; break; 103 | case m5::imu_bmi270: imu_name = "bmi270"; break; 104 | default: imu_name = "unknown"; break; 105 | }; 106 | 107 | if (imu_type == m5::imu_none) { 108 | for (;;) { 109 | delay(1); 110 | } 111 | } 112 | 113 | // Read calibration values from NVS. 114 | if (!M5.Imu.loadOffsetFromNVS()) { 115 | Serial.println("Could not load calibration data!"); 116 | } 117 | 118 | gyroX.clear(); 119 | gyroY.clear(); 120 | gyroZ.clear(); 121 | 122 | accelX.clear(); 123 | accelY.clear(); 124 | accelZ.clear(); 125 | 126 | delay(200); 127 | last_update = micros(); 128 | } 129 | 130 | void loop(void) { 131 | M5.update(); 132 | repeatMe(); 133 | delayMicroseconds(3000); 134 | } 135 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-init.tex-part: -------------------------------------------------------------------------------- 1 | \section{Initialization and Reference Alignment} 2 | 3 | Accurate initialization of the filter state is essential for rapid convergence 4 | and numerical stability. The procedure aligns the quaternion, biases, and 5 | latent acceleration with physically observable quantities from the 6 | accelerometer and magnetometer before dynamic updates commence. 7 | 8 | \subsection{Attitude Initialization from Accelerometer} 9 | 10 | At rest, the accelerometer measures the apparent specific force 11 | \( 12 | \vct{f}_b \approx -\mat{R}_{wb}\,\vct{g}_w, 13 | \) 14 | where $\vct{g}_w = [0,\,0,\,g]^\top$ is the gravity vector in world 15 | coordinates. The normalized accelerometer output 16 | \[ 17 | \hat{\vct{f}}_b 18 | = \frac{\vct{f}_b}{\|\vct{f}_b\|} 19 | \] 20 | defines the direction of gravity in the body frame. 21 | The initial world-to-body quaternion $q_0$ is chosen such that 22 | \[ 23 | \mat{R}(q_0)^\top\,\hat{\vct{f}}_b = -\vct{e}_z, 24 | \] 25 | that is, the body $z$-axis is aligned with the measured gravity vector. 26 | This provides the initial roll and pitch angles, while yaw remains 27 | undetermined until magnetometer data are available. 28 | If the initial condition is not strictly static, an averaged 29 | low-pass filtered accelerometer reading is used to suppress transient 30 | dynamic components. 31 | 32 | \subsection{Yaw Initialization from Magnetometer} 33 | 34 | The magnetometer provides a heading reference by observing the local 35 | magnetic field~$\vct{B}_w$ expressed in world coordinates. 36 | Given the measured field $\vct{m}_b$ in the body frame and the 37 | known inclination~$I$ and declination~$D$, the horizontal projection is 38 | \[ 39 | \vct{m}_{b,xy} = 40 | \begin{bmatrix} m_x \\[2pt] m_y \end{bmatrix}, \qquad 41 | \psi = \mathrm{atan2}(-m_y,\,m_x) - D. 42 | \] 43 | The initial quaternion is then completed by a rotation about the world 44 | $z$-axis through angle~$\psi$, appended to the tilt-corrected orientation: 45 | \[ 46 | q_0 \leftarrow q_{\mathrm{acc}} \otimes q_z(\psi). 47 | \] 48 | This establishes full three-axis attitude alignment consistent with 49 | local magnetic north. 50 | 51 | \subsection{Bias and Latent State Initialization} 52 | 53 | Gyroscope and accelerometer biases are initialized from pre-sampling averages: 54 | \[ 55 | \vct{b}_{g0} = \mathrm{mean}(\vct{\omega}_b), \qquad 56 | \vct{b}_{a0} = \mathrm{mean}(\vct{f}_b) + \mat{R}(q_0)\,\vct{g}_w. 57 | \] 58 | The latent OU acceleration $\vct{a}_{w0}$ is set to zero or to the 59 | gravity-compensated average of recent acceleration samples, providing a 60 | smooth start for the translational chain: 61 | \[ 62 | \vct{a}_{w0} = \mat{R}(q_0)^\top 63 | \bigl(\mathrm{mean}(\vct{f}_b) - \vct{b}_{a0}\bigr) + \vct{g}_w. 64 | \] 65 | All linear states $(\vct{v},\vct{p},\vct{S})$ are initialized to 66 | zero unless prior motion information is available. 67 | 68 | \subsection{Covariance Initialization} 69 | 70 | The initial covariance~$\mat{P}_0$ is block-diagonal, reflecting independent 71 | uncertainties in attitude, biases, and linear motion: 72 | \[ 73 | \mat{P}_0 = 74 | \diag\!\big( 75 | \matg{\Sigma}_{\theta0},\, 76 | \matg{\Sigma}_{b_g0},\, 77 | \matg{\Sigma}_{v0},\, 78 | \matg{\Sigma}_{p0},\, 79 | \matg{\Sigma}_{S0},\, 80 | \matg{\Sigma}_{a_w0},\, 81 | \matg{\Sigma}_{b_a0}\big). 82 | \] 83 | Typical values for $\matg{\Sigma}_{\theta0}$ correspond to several degrees of 84 | tilt uncertainty, while $\matg{\Sigma}_{a_w0}$ is set to the stationary variance 85 | of the OU process $\sigma_{a_w}^2$. 86 | This initialization ensures positive definiteness of~$\mat{P}_0$ and allows 87 | the filter to transition smoothly from the stationary prior to fully 88 | dynamic operation. 89 | 90 | \subsection{Alignment Verification and Warm-Up} 91 | 92 | During the first few seconds of operation, a warm-up phase validates the 93 | attitude alignment by monitoring the magnitude of the accelerometer residual. 94 | If the residual remains within tolerance, magnetic updates are enabled and 95 | the filter transitions to full fusion. This staged activation prevents 96 | spurious yaw correction during initial transients and establishes a stable 97 | baseline for subsequent online adaptation. 98 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-mnoise.tex-part: -------------------------------------------------------------------------------- 1 | \section{Measurement Noise and Covariance Structure} 2 | 3 | Each observation channel introduces measurement noise with distinct physical 4 | origins and statistical characteristics. These noises are modeled as 5 | independent zero-mean Gaussian processes, and their covariance matrices 6 | are maintained explicitly within the filter to ensure consistent weighting 7 | across sensing modalities. 8 | 9 | \subsection{Accelerometer Measurement Covariance} 10 | 11 | The accelerometer measures the specific force acting on the sensor in the 12 | body frame. Random errors arise from sensor quantization, scale-factor 13 | mismatch, and mechanical noise. The corresponding noise model is 14 | \[ 15 | \mathbf{n}_a \sim \mathcal{N}(\mathbf{0},\,\mathbf{R}_a), 16 | \qquad 17 | \mathbf{R}_a = \mathrm{diag}(\sigma_{ax}^2,\,\sigma_{ay}^2,\,\sigma_{az}^2). 18 | \] 19 | This diagonal covariance determines the relative confidence in each body 20 | axis and directly scales the innovation covariance for the accelerometer 21 | update. The entries are expressed in squared physical units 22 | $(\mathrm{m/s^2})^2$ and are typically derived from static noise floor 23 | characterization of the IMU. 24 | 25 | \subsection{Magnetometer Measurement Covariance} 26 | 27 | The magnetometer provides an external yaw reference through observation of 28 | the Earth’s magnetic field. Sensor errors include thermal drift and local 29 | field disturbances. The noise is modeled as 30 | \[ 31 | \mathbf{n}_m \sim \mathcal{N}(\mathbf{0},\,\mathbf{R}_m), 32 | \qquad 33 | \mathbf{R}_m = \mathrm{diag}(\sigma_{mx}^2,\,\sigma_{my}^2,\,\sigma_{mz}^2). 34 | \] 35 | The magnetometer update uses this covariance when forming the innovation 36 | matrix 37 | \( 38 | \mathbf{S}_m = J_\theta P_{\theta\theta} J_\theta^\top + \mathbf{R}_m, 39 | \) 40 | ensuring proper normalization of heading corrections. 41 | 42 | \subsection{Integral Drift Constraint Covariance} 43 | 44 | To stabilize long-term displacement drift, a pseudo-measurement constrains 45 | the integral of position~$\mathbf{S}$ toward zero. Its noise covariance 46 | \[ 47 | \mathbf{R}_S 48 | = \mathrm{diag}(\sigma_{Sx}^2,\,\sigma_{Sy}^2,\,\sigma_{Sz}^2) 49 | \] 50 | sets the effective stiffness of this constraint: smaller values yield 51 | stronger suppression of low-frequency drift, while larger values preserve 52 | freer motion at very long periods. The pseudo-measurement enters the update 53 | as 54 | \( 55 | \mathbf{S}_S = P_{SS} + \mathbf{R}_S. 56 | \) 57 | 58 | \subsection{Composite Measurement Structure} 59 | 60 | Although each measurement type is processed independently, all share a 61 | common structure for residual formation and covariance normalization. 62 | For any observation 63 | \( 64 | \mathbf{z} = h(\mathbf{x}) + \mathbf{n}, 65 | \) 66 | the linearized update uses 67 | \begin{align} 68 | \mathbf{r} &= \mathbf{z} - h(\hat{\mathbf{x}}_{k|k-1}),\\ 69 | \mathbf{S} &= \mathbf{C}\,P_{k|k-1}\,\mathbf{C}^\top + \mathbf{R},\\ 70 | \mathbf{K} &= P_{k|k-1}\,\mathbf{C}^\top\,\mathbf{S}^{-1}, 71 | \end{align} 72 | where $\mathbf{R}$ corresponds to $\mathbf{R}_a$, $\mathbf{R}_m$, or 73 | $\mathbf{R}_S$ depending on the active measurement. 74 | The corrected covariance is obtained using the Joseph symmetric form, 75 | \[ 76 | P_{k|k} = 77 | (\mathbf{I}-\mathbf{K}\mathbf{C})\,P_{k|k-1}\, 78 | (\mathbf{I}-\mathbf{K}\mathbf{C})^\top 79 | + \mathbf{K}\mathbf{R}\mathbf{K}^\top, 80 | \] 81 | which guarantees $P_{k|k}$ remains positive semidefinite even under 82 | finite-precision arithmetic. 83 | 84 | \subsection{Measurement Weighting and Observability Balance} 85 | 86 | Relative magnitudes of $\mathbf{R}_a$, $\mathbf{R}_m$, and $\mathbf{R}_S$ 87 | determine how strongly each sensor influences the estimation: 88 | \begin{itemize} 89 | \item small $\mathbf{R}_a$ emphasizes short-term attitude correction and 90 | stabilization of roll and pitch; 91 | \item small $\mathbf{R}_m$ enforces magnetic heading alignment; 92 | \item small $\mathbf{R}_S$ constrains long-term integral drift. 93 | \end{itemize} 94 | Proper tuning of these covariances balances observability between 95 | inertial and environmental cues and preserves stability across different 96 | sea-state regimes and motion intensities. 97 | 98 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_jonswap.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import matplotlib.pyplot as plt 3 | 4 | # Filenames and wave type colors 5 | files = { 6 | 'Short waves': 'short_waves_stokes.csv', 7 | 'Medium waves': 'medium_waves_stokes.csv', 8 | 'Long waves': 'long_waves_stokes.csv', 9 | } 10 | 11 | # Colors for each wave type and component (x,y,z) 12 | wave_colors = { 13 | 'Short waves': ['#e41a1c', '#a50f15', '#fb6a4a'], # Reds 14 | 'Medium waves': ['#4daf4a', '#2b7a2b', '#a1d99b'], # Greens 15 | 'Long waves': ['#377eb8', '#184f7d', '#9ecae1'], # Blues 16 | } 17 | 18 | # === Existing charts: Displacement, Velocity, Acceleration (world frame) === 19 | components = { 20 | 'Displacement': ['disp_x', 'disp_y', 'disp_z'], 21 | 'Velocity': ['vel_x', 'vel_y', 'vel_z'], 22 | 'Acceleration': ['acc_x', 'acc_y', 'acc_z'], 23 | } 24 | 25 | fig, axes = plt.subplots(len(components), 1, figsize=(14, 10), sharex=True) 26 | fig.suptitle('Wave Simulation Data from Jonswap3dStokesWaves') 27 | 28 | for wave_label, filename in files.items(): 29 | data = pd.read_csv(filename) 30 | time = data['time'] 31 | colors = wave_colors[wave_label] 32 | 33 | for ax, (comp_label, cols) in zip(axes, components.items()): 34 | for col_name, color in zip(cols, colors): 35 | ax.plot(time, data[col_name], label=f'{wave_label} {col_name}', color=color, alpha=0.8) 36 | ax.set_ylabel(comp_label) 37 | ax.grid(True) 38 | 39 | axes[-1].set_xlabel('Time [s]') 40 | 41 | # Custom legend without duplicates 42 | handles, labels = [], [] 43 | for ax in axes: 44 | h, l = ax.get_legend_handles_labels() 45 | handles.extend(h) 46 | labels.extend(l) 47 | seen = set() 48 | unique = [] 49 | for h, l in zip(handles, labels): 50 | if l not in seen: 51 | unique.append((h, l)) 52 | seen.add(l) 53 | handles, labels = zip(*unique) 54 | axes[0].legend(handles, labels, loc='upper right', fontsize='small', ncol=3) 55 | 56 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 57 | 58 | # === New chart 1: IMU body-frame acceleration === 59 | fig1, ax1 = plt.subplots(3, 1, figsize=(14, 8), sharex=True) 60 | fig1.suptitle('IMU Body-frame Acceleration') 61 | 62 | for wave_label, filename in files.items(): 63 | data = pd.read_csv(filename) 64 | time = data['time'] 65 | colors = wave_colors[wave_label] 66 | for i, comp in enumerate(['accel_x', 'accel_y', 'accel_z']): 67 | ax1[i].plot(time, data[comp], label=f'{wave_label} {comp}', color=colors[i], alpha=0.8) 68 | ax1[i].set_ylabel(comp) 69 | ax1[i].grid(True) 70 | 71 | ax1[-1].set_xlabel('Time [s]') 72 | ax1[0].legend(loc='upper right', fontsize='small', ncol=3) 73 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 74 | 75 | # === New chart 2: IMU gyro === 76 | fig2, ax2 = plt.subplots(3, 1, figsize=(14, 8), sharex=True) 77 | fig2.suptitle('IMU Gyro (Angular Velocity)') 78 | 79 | for wave_label, filename in files.items(): 80 | data = pd.read_csv(filename) 81 | time = data['time'] 82 | colors = wave_colors[wave_label] 83 | for i, comp in enumerate(['gyro_x', 'gyro_y', 'gyro_z']): 84 | ax2[i].plot(time, data[comp], label=f'{wave_label} {comp}', color=colors[i], alpha=0.8) 85 | ax2[i].set_ylabel(comp) 86 | ax2[i].grid(True) 87 | 88 | ax2[-1].set_xlabel('Time [s]') 89 | ax2[0].legend(loc='upper right', fontsize='small', ncol=3) 90 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 91 | 92 | # === New chart 3: Euler angles === 93 | fig3, ax3 = plt.subplots(3, 1, figsize=(14, 8), sharex=True) 94 | fig3.suptitle('IMU Euler Angles (degrees, constrained yaw)') 95 | 96 | for wave_label, filename in files.items(): 97 | data = pd.read_csv(filename) 98 | time = data['time'] 99 | colors = wave_colors[wave_label] 100 | for i, comp in enumerate(['roll_deg', 'pitch_deg', 'yaw_deg']): 101 | ax3[i].plot(time, data[comp], label=f'{wave_label} {comp}', color=colors[i], alpha=0.8) 102 | ax3[i].set_ylabel(comp) 103 | ax3[i].grid(True) 104 | 105 | ax3[-1].set_xlabel('Time [s]') 106 | ax3[0].legend(loc='upper right', fontsize='small', ncol=3) 107 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 108 | 109 | plt.show() 110 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_pm_stokes.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import matplotlib.pyplot as plt 3 | 4 | # Filenames and wave type colors for PM Stokes waves 5 | files = { 6 | 'Short PM waves': 'short_pms_waves.csv', 7 | 'Medium PM waves': 'medium_pms_waves.csv', 8 | 'Long PM waves': 'long_pms_waves.csv', 9 | } 10 | 11 | # Colors for each wave type and component (x,y,z) 12 | wave_colors = { 13 | 'Short PM waves': ['#e41a1c', '#a50f15', '#fb6a4a'], # Reds 14 | 'Medium PM waves': ['#4daf4a', '#2b7a2b', '#a1d99b'], # Greens 15 | 'Long PM waves': ['#377eb8', '#184f7d', '#9ecae1'], # Blues 16 | } 17 | 18 | # === Existing charts: Displacement, Velocity, Acceleration (world frame) === 19 | components = { 20 | 'Displacement': ['disp_x', 'disp_y', 'disp_z'], 21 | 'Velocity': ['vel_x', 'vel_y', 'vel_z'], 22 | 'Acceleration': ['acc_x', 'acc_y', 'acc_z'], 23 | } 24 | 25 | fig, axes = plt.subplots(len(components), 1, figsize=(14, 10), sharex=True) 26 | fig.suptitle('Wave Simulation Data from PMStokesN3dWaves') 27 | 28 | for wave_label, filename in files.items(): 29 | data = pd.read_csv(filename) 30 | time = data['time'] 31 | colors = wave_colors[wave_label] 32 | 33 | for ax, (comp_label, cols) in zip(axes, components.items()): 34 | for col_name, color in zip(cols, colors): 35 | ax.plot(time, data[col_name], label=f'{wave_label} {col_name}', color=color, alpha=0.8) 36 | ax.set_ylabel(comp_label) 37 | ax.grid(True) 38 | 39 | axes[-1].set_xlabel('Time [s]') 40 | 41 | # Custom legend without duplicates 42 | handles, labels = [], [] 43 | for ax in axes: 44 | h, l = ax.get_legend_handles_labels() 45 | handles.extend(h) 46 | labels.extend(l) 47 | seen = set() 48 | unique = [] 49 | for h, l in zip(handles, labels): 50 | if l not in seen: 51 | unique.append((h, l)) 52 | seen.add(l) 53 | handles, labels = zip(*unique) 54 | axes[0].legend(handles, labels, loc='upper right', fontsize='small', ncol=3) 55 | 56 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 57 | 58 | # === New chart 1: IMU body-frame acceleration === 59 | fig1, ax1 = plt.subplots(3, 1, figsize=(14, 8), sharex=True) 60 | fig1.suptitle('IMU Body-frame Acceleration (PM Waves)') 61 | 62 | for wave_label, filename in files.items(): 63 | data = pd.read_csv(filename) 64 | time = data['time'] 65 | colors = wave_colors[wave_label] 66 | for i, comp in enumerate(['accel_x', 'accel_y', 'accel_z']): 67 | ax1[i].plot(time, data[comp], label=f'{wave_label} {comp}', color=colors[i], alpha=0.8) 68 | ax1[i].set_ylabel(comp) 69 | ax1[i].grid(True) 70 | 71 | ax1[-1].set_xlabel('Time [s]') 72 | ax1[0].legend(loc='upper right', fontsize='small', ncol=3) 73 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 74 | 75 | # === New chart 2: IMU gyro === 76 | fig2, ax2 = plt.subplots(3, 1, figsize=(14, 8), sharex=True) 77 | fig2.suptitle('IMU Gyro (Angular Velocity, PM Waves)') 78 | 79 | for wave_label, filename in files.items(): 80 | data = pd.read_csv(filename) 81 | time = data['time'] 82 | colors = wave_colors[wave_label] 83 | for i, comp in enumerate(['gyro_x', 'gyro_y', 'gyro_z']): 84 | ax2[i].plot(time, data[comp], label=f'{wave_label} {comp}', color=colors[i], alpha=0.8) 85 | ax2[i].set_ylabel(comp) 86 | ax2[i].grid(True) 87 | 88 | ax2[-1].set_xlabel('Time [s]') 89 | ax2[0].legend(loc='upper right', fontsize='small', ncol=3) 90 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 91 | 92 | # === New chart 3: Euler angles === 93 | fig3, ax3 = plt.subplots(3, 1, figsize=(14, 8), sharex=True) 94 | fig3.suptitle('IMU Euler Angles (degrees, constrained yaw, PM Waves)') 95 | 96 | for wave_label, filename in files.items(): 97 | data = pd.read_csv(filename) 98 | time = data['time'] 99 | colors = wave_colors[wave_label] 100 | for i, comp in enumerate(['roll_deg', 'pitch_deg', 'yaw_deg']): 101 | ax3[i].plot(time, data[comp], label=f'{wave_label} {comp}', color=colors[i], alpha=0.8) 102 | ax3[i].set_ylabel(comp) 103 | ax3[i].grid(True) 104 | 105 | ax3[-1].set_xlabel('Time [s]') 106 | ax3[0].legend(loc='upper right', fontsize='small', ncol=3) 107 | plt.tight_layout(rect=[0, 0, 1, 0.95]) 108 | 109 | plt.show() 110 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/kalman3d_refs.bib: -------------------------------------------------------------------------------- 1 | @book{jazwinski1970, 2 | author = {Andrew H. Jazwinski}, 3 | title = {Stochastic Processes and Filtering Theory}, 4 | publisher = {Academic Press}, 5 | year = {1970}, 6 | address = {New York}, 7 | isbn = {9780123815505}, 8 | note = {Foundational reference for continuous and discrete Kalman filtering theory.} 9 | } 10 | 11 | @book{maybeck1979, 12 | author = {Peter S. Maybeck}, 13 | title = {Stochastic Models, Estimation, and Control, Volume 1}, 14 | publisher = {Academic Press}, 15 | year = {1979}, 16 | address = {New York}, 17 | isbn = {9780124807011}, 18 | note = {Defines Joseph form and canonical discrete-time Kalman filter equations.} 19 | } 20 | 21 | @book{crassidis2012, 22 | author = {John L. Crassidis and John L. Junkins}, 23 | title = {Optimal Estimation of Dynamic Systems}, 24 | edition = {2nd}, 25 | publisher = {CRC Press}, 26 | year = {2012}, 27 | address = {Boca Raton}, 28 | isbn = {9781439839850}, 29 | note = {Primary reference for attitude estimation and multiplicative EKF formulations.} 30 | } 31 | 32 | @book{brown2012, 33 | author = {Robert G. Brown and Patrick Y. C. Hwang}, 34 | title = {Introduction to Random Signals and Applied Kalman Filtering}, 35 | edition = {4th}, 36 | publisher = {Wiley}, 37 | year = {2012}, 38 | address = {Hoboken, NJ}, 39 | isbn = {9781118582557}, 40 | note = {Practical OU process modeling and covariance discretization techniques.} 41 | } 42 | 43 | @article{vanloan1978integrating, 44 | author = {Charles F. Van Loan}, 45 | title = {Computing Integrals Involving the Matrix Exponential}, 46 | journal = {IEEE Transactions on Automatic Control}, 47 | volume = {23}, 48 | number = {3}, 49 | pages = {395--404}, 50 | year = {1978}, 51 | doi = {10.1109/TAC.1978.1101743}, 52 | note = {Defines analytic form of $Q_d$ integral used in discrete-time Kalman filters.} 53 | } 54 | 55 | @article{uhlenbeck1930, 56 | author = {G. E. Uhlenbeck and L. S. Ornstein}, 57 | title = {On the Theory of the Brownian Motion}, 58 | journal = {Physical Review}, 59 | volume = {36}, 60 | pages = {823--841}, 61 | year = {1930}, 62 | doi = {10.1103/PhysRev.36.823}, 63 | note = {Original introduction of the Ornstein–Uhlenbeck process.} 64 | } 65 | 66 | @article{shuster1993, 67 | author = {Malcolm D. Shuster}, 68 | title = {A Survey of Attitude Representations}, 69 | journal = {Journal of the Astronautical Sciences}, 70 | volume = {41}, 71 | number = {4}, 72 | pages = {439--517}, 73 | year = {1993}, 74 | note = {Defines quaternion small-angle error parameterization and attitude error kinematics.} 75 | } 76 | 77 | @article{markley2003, 78 | author = {F. Landis Markley and Yang Cheng and John L. Crassidis and Yaakov Oshman}, 79 | title = {Quaternion Averaging}, 80 | journal = {Journal of Guidance, Control, and Dynamics}, 81 | volume = {30}, 82 | number = {4}, 83 | pages = {1193--1197}, 84 | year = {2007}, 85 | doi = {10.2514/1.28949}, 86 | note = {Standard reference for quaternion update and normalization logic in multiplicative filters.} 87 | } 88 | 89 | @article{sontag1996, 90 | author = {Eduardo D. Sontag and Yuan Wang}, 91 | title = {New Characterizations of Input-to-State Stability}, 92 | journal = {IEEE Transactions on Automatic Control}, 93 | volume = {41}, 94 | number = {9}, 95 | pages = {1283--1294}, 96 | year = {1996}, 97 | doi = {10.1109/9.536530}, 98 | note = {Fundamental ISS theory applied in adaptive stability proofs.} 99 | } 100 | 101 | @article{jiang1997, 102 | author = {Z. P. Jiang and Iven M. Y. Mareels}, 103 | title = {A Small-Gain Control Method for Nonlinear Systems}, 104 | journal = {IEEE Transactions on Automatic Control}, 105 | volume = {42}, 106 | number = {3}, 107 | pages = {386--398}, 108 | year = {1997}, 109 | doi = {10.1109/9.557552}, 110 | note = {Used for ISS and Lyapunov stability argumentation in adaptive filters.} 111 | } 112 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-ou-q.tex-part: -------------------------------------------------------------------------------- 1 | \section*{Appendix B.9: Explicit, Cancellation-Safe Formulas for $Q_d^{(1)}$} 2 | \label{app:ou-qtable} 3 | 4 | We give closed-form expressions for the per-axis discrete process covariance 5 | \[ 6 | Q_d^{(1)}(h,\tau,\sigma_a^2) 7 | = \frac{2\sigma_a^2}{\tau}\int_{0}^{h} c(s)c(s)^\top ds, 8 | \qquad 9 | c(s)= 10 | \begin{bmatrix} 11 | \phi_{va}(s)\\ \phi_{pa}(s)\\ \phi_{Sa}(s)\\ e^{-s/\tau} 12 | \end{bmatrix}, 13 | \] 14 | where, for each \(s\in[0,h]\), 15 | \[ 16 | \phi_{va}(s)=\tau\!\left(1-e^{-s/\tau}\right),\quad 17 | \phi_{pa}(s)=\tau s+\tau^2\!\left(e^{-s/\tau}-1\right),\quad 18 | \phi_{Sa}(s)=\tfrac12 s^2-\tau s+\tau^2\!\left(1-e^{-s/\tau}\right). 19 | \] 20 | 21 | \paragraph{Primitives (stable for $x=h/\tau$ small).} 22 | Let \(x=h/\tau\), \(E_1=e^{-x}\), \(E_2=e^{-2x}\). Define 23 | \[ 24 | A_0 := \tau(1-E_1),\qquad 25 | B_0 := \tfrac{\tau}{2}(1-E_2), 26 | \] 27 | \[ 28 | I_1 := \int_{0}^{h} s e^{-s/\tau} ds 29 | = \tau^2\!\left[1 - E_1(1+x)\right], 30 | \] 31 | \[ 32 | I_2 := \int_{0}^{h} s^2 e^{-s/\tau} ds 33 | = \tau^3\!\left[2 - E_1\,(2+2x+x^2)\right]. 34 | \] 35 | Elementary polynomials: 36 | \[ 37 | \int_0^h s\,ds=\tfrac12 h^2,\quad 38 | \int_0^h s^2\,ds=\tfrac13 h^3,\quad 39 | \int_0^h s^3\,ds=\tfrac14 h^4,\quad 40 | \int_0^h s^4\,ds=\tfrac15 h^5. 41 | \] 42 | Also 43 | \[ 44 | \int_0^h (1-e^{-s/\tau})\,ds = h - A_0,\quad 45 | \int_0^h (1-e^{-s/\tau})^2 ds = h - 2A_0 + B_0, 46 | \] 47 | \[ 48 | \int_0^h s(1-e^{-s/\tau}) ds = \tfrac12 h^2 - I_1,\quad 49 | \int_0^h s^2(1-e^{-s/\tau}) ds = \tfrac13 h^3 - I_2. 50 | \] 51 | 52 | \paragraph{Entries of $Q_d^{(1)}$ (order $[v,p,S,a]$).} 53 | With \(q:=\tfrac{2\sigma_a^2}{\tau}\), the entries below give the full matrix; 54 | use symmetry \(Q_{ij}=Q_{ji}\). 55 | 56 | \begin{align*} 57 | Q_{aa} &= q\,\underbrace{\int_0^h e^{-2s/\tau}\,ds}_{=\,B_0} 58 | \;=\; \boxed{\sigma_a^2\,(1-E_2)}.\\[4pt] 59 | Q_{va} &= q\,\int_0^h \phi_{va}(s)\,e^{-s/\tau} ds 60 | \;=\; \boxed{2\sigma_a^2\,(A_0 - B_0)} 61 | \;=\; \boxed{\sigma_a^2\,\tau\,(1-E_1)^2}.\\[4pt] 62 | Q_{vv} &= q\,\int_0^h \phi_{va}(s)^2 ds 63 | \;=\; \boxed{2\sigma_a^2\,\tau\Big(h - 2A_0 + B_0\Big)}.\\[6pt] 64 | Q_{pa} &= q\,\int_0^h \phi_{pa}(s)\,e^{-s/\tau} ds 65 | \;=\; \boxed{2\sigma_a^2\Big(I_1 + \tau(B_0 - A_0)\Big)}.\\[4pt] 66 | Q_{pv} &= q\,\int_0^h \phi_{pa}(s)\,\phi_{va}(s) ds 67 | \;=\; \boxed{\frac{2\sigma_a^2}{\tau}\Big(\tau^2(\tfrac12 h^2 - I_1) - \tau^3(h - 2A_0 + B_0)\Big)}\\ 68 | &= \boxed{2\sigma_a^2\Big(\tfrac12 \tau h^2 - \tfrac{I_1}{\ } - \tau^2(h - 2A_0 + B_0)\Big)}.\\[6pt] 69 | Q_{pp} &= q\,\int_0^h \phi_{pa}(s)^2 ds \\ 70 | &= \boxed{\frac{2\sigma_a^2}{\tau}\Big(\tau^2\tfrac{h^3}{3} + 2\tau^3(I_1 - \tfrac12 h^2) + \tau^4(h - 2A_0 + B_0)\Big)}.\\[6pt] 71 | Q_{Sa} &= q\,\int_0^h \phi_{Sa}(s)\,e^{-s/\tau} ds 72 | \;=\; \boxed{\frac{2\sigma_a^2}{\tau}\Big(\tfrac12 I_2 - \tau I_1 + \tau^2(A_0 - B_0)\Big)}.\\[6pt] 73 | Q_{Sv} &= q\,\int_0^h \phi_{Sa}(s)\,\phi_{va}(s) ds \\ 74 | &= \boxed{\frac{2\sigma_a^2}{\tau}\Big(\tfrac{\tau}{2}(\tfrac{h^3}{3} - I_2) - \tau^2(\tfrac12 h^2 - I_1) + \tau^3(h - 2A_0 + B_0)\Big)}.\\[6pt] 75 | Q_{Sp} &= q\,\int_0^h \phi_{Sa}(s)\,\phi_{pa}(s) ds \\ 76 | &= \boxed{\frac{2\sigma_a^2}{\tau}\Big(\tfrac{\tau}{8}h^4 + \tau^2(\tfrac12 I_2 - \tfrac12 h^3) + \tau^3(h^2 - 2 I_1) - \tau^4(h - 2A_0 + B_0)\Big)}.\\[6pt] 77 | Q_{SS} &= q\,\int_0^h \phi_{Sa}(s)^2 ds \\ 78 | &= \boxed{\frac{2\sigma_a^2}{\tau}\Big(\tfrac{1}{20}h^5 - \tfrac{\tau}{4}h^4 + \tau^2(\tfrac{2}{3}h^3 - I_2) - \tau^3 h^2 + 2\tau^3 I_1 + \tau^4(h - 2A_0 + B_0)\Big)}. 79 | \end{align*} 80 | 81 | \paragraph{Checks and small-$x$ series.} 82 | As \(x\to 0\), the series of each entry matches the Maclaurin branch used in 83 | the implementation; e.g., 84 | \[ 85 | Q_{aa}=\sigma_a^2(2x - 2x^2 + \tfrac{4}{3}x^3 + \cdots),\quad 86 | Q_{va}=\sigma_a^2\tau\!\left(\tfrac{x^2}{1} - \tfrac{2}{3}x^3 + \tfrac{7}{12}x^4 + \cdots\right), 87 | \] 88 | \[ 89 | Q_{vv}=2\sigma_a^2\tau\!\left(h - \tau x + \tfrac{\tau}{2}x^2 - \cdots\right) 90 | =\sigma_a^2\!\left(\tfrac{2}{3}h^3 - \tfrac{1}{2}\tfrac{h^4}{\tau} + \tfrac{1}{5}\tfrac{h^5}{\tau^2} + \cdots\right), 91 | \] 92 | and similarly for the remaining entries when substituting \(A_0,B_0,I_1,I_2\) by 93 | their series. This confirms numerical consistency with the analytic 94 | \textit{and} Maclaurin branches. 95 | 96 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-phi-symb.tex-part: -------------------------------------------------------------------------------- 1 | \subsection{Discrete Transition $\Phi(h)=e^{Fh}$ from the Block $F$ (One Axis)} 2 | 3 | \paragraph{Model and block structure.} 4 | For a single Cartesian axis we stack 5 | \[ 6 | \boldsymbol{x}_{\text{axis}}(t)=\begin{bmatrix} v(t) & p(t) & S(t) & a(t)\end{bmatrix}^{\!\top}, 7 | \] 8 | with continuous LTI dynamics 9 | \[ 10 | \begin{aligned} 11 | \dot{\boldsymbol{x}}_{\text{axis}}(t)&= 12 | \underbrace{\begin{bmatrix}A&B\\[2pt]0&D\end{bmatrix}}_{F} 13 | \boldsymbol{x}_{\text{axis}}(t),\qquad\\ 14 | A=\begin{bmatrix}0&0&0\\ 1&0&0\\ 0&1&0\end{bmatrix},\quad 15 | B&=\begin{bmatrix}1\\0\\0\end{bmatrix},\quad 16 | D=-\frac{1}{\tau}. 17 | \end{aligned} 18 | \] 19 | Here $(v,p,S)$ form the kinematic chain driven by $a$; the OU decay of $a$ is $D$. 20 | Note that $A^3=0$ (nilpotent). 21 | 22 | \paragraph{Block–triangular exponential.} 23 | Because $F$ is block upper triangular, 24 | \[ 25 | \begin{aligned} 26 | e^{F s}&= 27 | \begin{bmatrix} 28 | e^{A s} & \displaystyle\int_{0}^{s} e^{A(s-u)}\,B\,e^{D u}\,du\\[6pt] 29 | 0 & e^{D s} 30 | \end{bmatrix} 31 | \quad\Rightarrow\quad\\ 32 | \Phi(h)=e^{Fh}&= 33 | \begin{bmatrix} 34 | e^{A h} & \displaystyle\int_{0}^{h} e^{A(h-u)}\,B\,e^{-u/\tau}\,du\\[6pt] 35 | 0 & e^{-h/\tau} 36 | \end{bmatrix} 37 | \end{aligned}. 38 | \] 39 | Since $A^3=0$, 40 | \[ 41 | e^{A s}=I + A s + \tfrac{1}{2}A^2 s^2 42 | = 43 | \begin{bmatrix} 44 | 1 & 0 & 0\\[2pt] 45 | s & 1 & 0\\[2pt] 46 | \tfrac{s^2}{2} & s & 1 47 | \end{bmatrix}. 48 | \] 49 | Moreover, $e^{A t}B = \begin{bmatrix}1\\ t\\ \tfrac{t^2}{2}\end{bmatrix}$. 50 | 51 | \paragraph{Top–right block \(\displaystyle\int_0^h e^{A(h-u)}B\,e^{-u/\tau}du\).} 52 | Define the \emph{feedthrough} vector 53 | \[ 54 | \begin{aligned} 55 | C_{\text{top}}(h)\;:&=\;\int_{0}^{h} e^{A(h-u)}\,B\,e^{-u/\tau}\,du\\ 56 | &=\int_{0}^{h} 57 | \begin{bmatrix} 58 | 1\\[2pt] 59 | h-u\\[2pt] 60 | \tfrac{(h-u)^2}{2} 61 | \end{bmatrix} 62 | e^{-u/\tau}\,du 63 | = 64 | \begin{bmatrix} 65 | \phi_{va}\\[2pt] \phi_{pa}\\[2pt] \phi_{Sa} 66 | \end{bmatrix} 67 | \end{aligned}. 68 | \] 69 | Introduce the dimensionless step \(x=\tfrac{h}{\tau}\) and OU primitives 70 | \[ 71 | \alpha=e^{-x},\qquad \mathrm{em1}=\alpha-1 \quad(\Rightarrow 1-\alpha=-\mathrm{em1}). 72 | \] 73 | Carrying out the three scalar integrals and simplifying to the compact combinations used 74 | in implementation yields the exact closed forms 75 | \[ 76 | \boxed{ 77 | \begin{aligned} 78 | \phi_{va} &= -\,\tau\,\mathrm{em1} 79 | \;=\;\tau(1-\alpha),\\[2pt] 80 | \phi_{pa} &= \tau^2\bigl(x+\mathrm{em1}\bigr) 81 | \;=\;\tau h-\tau^2(1-\alpha),\\[2pt] 82 | \phi_{Sa} &= \tau^3\!\left(\tfrac{1}{2}x^2 - x - \mathrm{em1}\right) 83 | \;=\;\tfrac{1}{2}\tau h^2 - \tau^2 h + \tau^3(1-\alpha). 84 | \end{aligned}} 85 | \] 86 | 87 | \paragraph{Bottom–right block.} 88 | \[ 89 | \phi_{aa} \;=\; e^{D h} \;=\; e^{-h/\tau}\;=\;\alpha. 90 | \] 91 | 92 | \paragraph{Assembled per–axis transition.} 93 | Combining the polynomial kinematics for $(v,p,S)$ with the feedthroughs above gives 94 | \[ 95 | \fitcol{ 96 | \boxed{ 97 | \Phi_{\text{axis}}(h)= 98 | \begin{bmatrix} 99 | 1 & 0 & 0 & \phi_{va}\\[2pt] 100 | h & 1 & 0 & \phi_{pa}\\[2pt] 101 | \tfrac{h^2}{2} & h & 1 & \phi_{Sa}\\[2pt] 102 | 0 & 0 & 0 & \alpha 103 | \end{bmatrix} 104 | = 105 | \begin{bmatrix} 106 | 1 & 0 & 0 & -\tau\,\mathrm{em1}\\[2pt] 107 | h & 1 & 0 & \tau^2(x+\mathrm{em1})\\[2pt] 108 | \tfrac{h^2}{2} & h & 1 & \tau^3\!\bigl(\tfrac{1}{2}x^2 - x - \mathrm{em1}\bigr)\\[2pt] 109 | 0 & 0 & 0 & \alpha 110 | \end{bmatrix}. 111 | } 112 | } 113 | \] 114 | 115 | \paragraph{Small–step asymptotics (\(x=h/\tau\ll 1\)).} 116 | For numerical robustness at high sampling rates, use the Maclaurin series 117 | \[ 118 | \alpha=1-x+\tfrac{x^2}{2}-\tfrac{x^3}{6}+\cdots,\qquad 119 | \mathrm{em1}=-x+\tfrac{x^2}{2}-\tfrac{x^3}{6}+\cdots, 120 | \] 121 | which imply 122 | \[ 123 | \begin{aligned} 124 | \phi_{va}&=\tau\!\left(x-\tfrac{x^2}{2}+\cdots\right)=h-\tfrac{h^2}{2\tau}+\cdots,\quad\\ 125 | \phi_{pa}&=\tau^2\!\left(\tfrac{x^2}{2}-\tfrac{x^3}{6}+\cdots\right)=\tfrac{h^2}{2}-\tfrac{h^3}{6\tau}+\cdots,\\ 126 | \phi_{Sa}&=\tau^3\!\left(\tfrac{x^3}{6}-\tfrac{x^4}{24}+\cdots\right)=\tfrac{h^3}{6}-\tfrac{h^4}{24\tau}+\cdots,\\ 127 | \alpha&=1-x+\tfrac{x^2}{2}-\cdots. 128 | \end{aligned} 129 | \] 130 | These cancel catastrophic subtraction in the \(x\to 0\) regime and match the 131 | branching used in code. 132 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-mupdate.tex-part: -------------------------------------------------------------------------------- 1 | \section{Measurement Update} 2 | 3 | After each prediction step, three complementary observation models are applied 4 | to correct attitude and translational drift. Each update operates on a 5 | linearized residual and uses the Joseph covariance form to preserve 6 | symmetry and positive semidefiniteness. 7 | 8 | \subsection{Accelerometer Update} 9 | 10 | The accelerometer provides a direct measurement of the specific force acting 11 | on the sensor in the body frame. The predicted value is obtained from the 12 | estimated orientation, latent world acceleration, and bias model, 13 | \begin{equation} 14 | \mathbf{f}_b^{\mathrm{pred}} 15 | = \mathbf{R}_{wb}(\mathbf{a}_w - \mathbf{g}_w) 16 | + \mathbf{b}_a(T), 17 | \end{equation} 18 | where $\mathbf{R}_{wb}$ is the world-to-body rotation, $\mathbf{g}_w$ is the 19 | gravity vector in world coordinates, and $\mathbf{b}_a(T)$ is the 20 | temperature-dependent accelerometer bias. 21 | The innovation is the difference between the measured and predicted specific 22 | forces, 23 | \[ 24 | \mathbf{r} = \mathbf{f}_b^{\mathrm{meas}} - \mathbf{f}_b^{\mathrm{pred}}. 25 | \] 26 | Linearization about the current state yields the measurement Jacobians 27 | \begin{align} 28 | \frac{\partial \mathbf{f}_b}{\partial \boldsymbol{\theta}} 29 | &= -[\mathbf{f}_b^{\mathrm{pred}}]_\times, & 30 | \frac{\partial \mathbf{f}_b}{\partial \mathbf{a}_w} 31 | &= \mathbf{R}_{wb}, & 32 | \frac{\partial \mathbf{f}_b}{\partial \mathbf{b}_a} 33 | &= \mathbf{I}_3, 34 | \end{align} 35 | which couple the measurement to attitude, OU acceleration, and bias states. 36 | The correction step follows the standard Kalman form using these Jacobians 37 | and the accelerometer noise covariance~$\mathbf{R}_a$. This update stabilizes 38 | roll and pitch and continuously re-anchors the latent acceleration process to 39 | the gravity direction. 40 | 41 | \subsection{Magnetometer Update} 42 | 43 | The magnetometer supplies an external heading reference by observing the 44 | local magnetic field. The predicted measurement is 45 | \[ 46 | \mathbf{m}_b^{\mathrm{pred}} = \mathbf{R}_{wb}\,\mathbf{B}_w, 47 | \] 48 | where $\mathbf{B}_w$ is the magnetic reference vector expressed in the world 49 | frame. The residual is computed as the difference between measured and 50 | predicted field vectors, corrected for sign ambiguity to ensure alignment in 51 | the same hemisphere. Linearization with respect to the attitude error gives 52 | \[ 53 | \frac{\partial \mathbf{m}_b}{\partial \boldsymbol{\theta}} 54 | = -[\mathbf{m}_b^{\mathrm{pred}}]_\times. 55 | \] 56 | This update refines yaw and heading without affecting translational states, 57 | closing the attitude triad defined by gravity and magnetic north. 58 | 59 | \subsection{Integral Drift Constraint} 60 | 61 | To control long-term drift in the triple-integrated displacement, a 62 | zero-valued pseudo-measurement is periodically applied to the integral state 63 | $\mathbf{S}$. The observation model is 64 | \begin{equation} 65 | \mathbf{z}_S = \mathbf{S} + \mathbf{n}_S, \qquad 66 | \mathbf{R}_S = \mathrm{diag}(\sigma_{Sx}^2,\,\sigma_{Sy}^2,\,\sigma_{Sz}^2). 67 | \end{equation} 68 | Its innovation $\mathbf{r} = -\mathbf{S}$ drives the integral toward zero 69 | while allowing low-frequency motion components to pass unaltered. 70 | This constraint acts as a weak stabilizer for cumulative drift in velocity and 71 | position and maintains bounded covariance in calm-sea conditions. 72 | 73 | \subsection{Update Cycle} 74 | 75 | Each observation channel executes the same normalized correction sequence: 76 | \begin{enumerate} 77 | \item Compute the measurement residual~$\mathbf{r}$. 78 | \item Linearize the observation model to form the Jacobian~$\mathbf{C}$. 79 | \item Evaluate the innovation covariance 80 | $\mathbf{S} = \mathbf{C} P \mathbf{C}^\top + \mathbf{R}$. 81 | \item Compute the Kalman gain 82 | $\mathbf{K} = P \mathbf{C}^\top \mathbf{S}^{-1}$. 83 | \item Apply the state correction 84 | $\mathbf{x} \leftarrow \mathbf{x} + \mathbf{K}\mathbf{r}$. 85 | \item Update the covariance using the Joseph form to maintain symmetry. 86 | \item Re-normalize the quaternion and clear the small-angle error component. 87 | \end{enumerate} 88 | This measurement cycle provides bounded corrections across attitude, bias, 89 | and translational channels, ensuring consistency between the OU-driven 90 | prediction model and physical sensor observations. 91 | 92 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/sliding_window_min_max.tex: -------------------------------------------------------------------------------- 1 | \documentclass[11pt,letterpaper]{article} 2 | \usepackage{amsmath,amssymb,amsthm,fullpage} 3 | \usepackage{hyperref} 4 | \usepackage{cite} 5 | 6 | \newtheorem{theorem}{Theorem} 7 | 8 | \title{Comprehensive Analysis of the Monotonic Wedge Algorithm for Sliding Window Extrema} 9 | \date{} 10 | 11 | \begin{document} 12 | \maketitle 13 | 14 | \begin{abstract} 15 | Sliding‐window extrema are critical in real‐time financial analysis, sensor networks, and streaming algorithms. Computing the minimum and maximum over a sliding window of length \(W\) in a data stream of length \(N\) is a fundamental operation in signal processing and time‐series analysis. The monotonic wedge algorithm maintains two deques (one for minima, one for maxima) in amortized \(O(1)\) time per update, with worst‐case \(O(\log W)\) via a hybrid search strategy. This paper presents a comprehensive analysis of its computational and memory usage complexity, including detailed memory‐utilization estimates, and compares it with alternative data structures. 16 | \end{abstract} 17 | 18 | \section{Problem Statement} 19 | Given a sequence \(\{x_k\}_{k=1}^N\) and a fixed window size \(W\), define 20 | \[ 21 | m_k = \min_{i=k-W+1}^k x_i,\qquad 22 | M_k = \max_{i=k-W+1}^k x_i, 23 | \] 24 | for \(k \ge W\). The goal is to update \(m_k\) and \(M_k\) online in sub-\(O(W)\) time per step. 25 | 26 | \section{Monotonic Wedge Algorithm} 27 | Maintain two deques: 28 | \[ 29 | \mathcal{D}_{\min},\quad \mathcal{D}_{\max}, 30 | \] 31 | each storing pairs \((x_j,j)\) in index order but with monotonic values: 32 | \[ 33 | \mathcal{D}_{\min}\colon x_{j_1}\le x_{j_2}\le\cdots,\quad 34 | \mathcal{D}_{\max}\colon x_{j_1}\ge x_{j_2}\ge\cdots. 35 | \] 36 | At each time \(k\): 37 | \begin{enumerate} 38 | \item \textbf{Prune back:}\\ 39 | \texttt{while\,(!D\_min.empty() \&\& D\_min.back().x > x\_k) D\_min.pop\_back();}\\ 40 | similarly for \texttt{D\_max} when its back value \([a-z]+)_H(?P[0-9.]+)_L(?P[0-9.]+)_A(?P[-0-9.]+)_P(?P

[-0-9.]+)\.csv$" 41 | ) 42 | 43 | def parse_filename(fname): 44 | stem = os.path.basename(fname) 45 | m = fname_re.match(stem) 46 | if not m: 47 | return None 48 | return { 49 | "wtype": m.group("wtype"), 50 | "H": float(m.group("H")), 51 | "L": float(m.group("L")), 52 | "A": float(m.group("A")), 53 | "P": float(m.group("P")), 54 | } 55 | 56 | def save_all(fig, base): 57 | """Save PGF + SVG + PNG (PNG needed for LaTeX sidecar images).""" 58 | fig.savefig(f"{base}.pgf", bbox_inches="tight", backend="pgf") 59 | fig.savefig(f"{base}.svg", bbox_inches="tight", dpi=150) 60 | # Disable LaTeX temporarily for PNG export to avoid dvipng dependency 61 | with mpl.rc_context({"text.usetex": False}): 62 | fig.savefig(f"{base}.png", bbox_inches="tight", dpi=300) 63 | print(f" saved {base}.pgf/.svg/.png") 64 | 65 | def make_plots(fname, group_label, meta): 66 | df = pd.read_csv(fname) 67 | freqs = np.sort(df["f_Hz"].unique()) 68 | thetas_deg = np.sort(df["theta_deg"].unique()) 69 | E = df.pivot(index="f_Hz", columns="theta_deg", values="E").values 70 | thetas_rad = np.deg2rad(thetas_deg) 71 | 72 | # --- Title with Hs and Azimuth --- 73 | title = ( 74 | fr"{meta['wtype'].capitalize()} spectrum " 75 | fr"($H_s={meta['H']:.2f}\,\mathrm{{m}},\ " 76 | fr"\alpha={meta['A']:.0f}^\circ$)" 77 | ) 78 | base = f"spectrum_{meta['wtype']}_{group_label}" 79 | 80 | # === Polar plot === 81 | R, T = np.meshgrid(freqs, thetas_rad) 82 | fig1, ax1 = plt.subplots(subplot_kw={'projection': 'polar'}) 83 | pcm = ax1.pcolormesh(T, R, E.T, shading='auto', cmap='viridis') 84 | pcm.set_rasterized(True) # rasterize dense mesh 85 | ax1.set_theta_zero_location("N") 86 | ax1.set_theta_direction(-1) 87 | ax1.set_title(title) 88 | plt.colorbar(pcm, ax=ax1, orientation="vertical", 89 | label=r"$E(f,\theta)\,[m^2/Hz/deg]$") 90 | save_all(fig1, f"{base}_polar") 91 | plt.close(fig1) 92 | 93 | # === 3D surface plot === 94 | T_grid, F_grid = np.meshgrid(thetas_deg, freqs) 95 | fig2 = plt.figure() 96 | ax2 = fig2.add_subplot(111, projection='3d') 97 | surf = ax2.plot_surface(F_grid, T_grid, E, 98 | cmap='viridis', linewidth=0, antialiased=True) 99 | surf.set_rasterized(True) 100 | ax2.set_xlabel("Frequency [Hz]") 101 | ax2.set_ylabel("Direction [deg]") 102 | ax2.set_zlabel(r"$E(f,\theta)$") 103 | ax2.set_title(title) 104 | fig2.colorbar(surf, shrink=0.5, aspect=10, 105 | label=r"$E(f,\theta)$") 106 | save_all(fig2, f"{base}_3d") 107 | plt.close(fig2) 108 | 109 | if __name__ == "__main__": 110 | files = glob.glob("wave_spectrum_*.csv") 111 | if not files: 112 | print("No wave_spectrum_*.csv files found.") 113 | exit() 114 | 115 | for fname in files: 116 | meta = parse_filename(fname) 117 | if not meta: 118 | continue 119 | for group, target_H in height_groups.items(): 120 | if abs(meta["H"] - target_H) < 1e-3: 121 | print(f"Processing {fname} as {group} ...") 122 | make_plots(fname, group, meta) 123 | 124 | print("All PGF, SVG, and PNG plots saved.") 125 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/tests/plot_sea_reg.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import pandas as pd 3 | import matplotlib.pyplot as plt 4 | import re 5 | import os 6 | import numpy as np 7 | 8 | # Directory where CSV files from harness are saved 9 | DATA_DIR = "./" # change if needed 10 | 11 | # Match C++ output: regularity___h.csv 12 | files = glob.glob(os.path.join(DATA_DIR, "regularity_*.csv")) 13 | 14 | pattern = re.compile( 15 | r"regularity_(?P[^_]+)_(?P[^_]+)_h(?P[0-9]+(?:\.[0-9]+)?)\.csv" 16 | ) 17 | 18 | # Map wave type to base color 19 | wave_colors = { 20 | "fenton": "Blues", 21 | "gerstner": "Purples", 22 | "jonswap": "Reds", 23 | "pmstokes": "Greens" 24 | } 25 | 26 | # Map wave type & height to target frequency 27 | # Must match your C++ waveParamsList 28 | wave_target_freq = { 29 | ("gerstner", "0.135"): 1.0/3.0, 30 | ("gerstner", "0.75"): 1.0/5.7, 31 | ("gerstner", "2"): 1.0/8.5, 32 | ("gerstner", "4.25"): 1.0/11.4, 33 | ("gerstner", "7.4"): 1.0/14.3, 34 | ("jonswap", "0.135"): 1.0/3.0, 35 | ("jonswap", "0.75"): 1.0/5.7, 36 | ("jonswap", "2"): 1.0/8.5, 37 | ("jonswap", "4.25"): 1.0/11.4, 38 | ("jonswap", "7.4"): 1.0/14.3, 39 | ("fenton", "0.135"): 1.0/3.0, 40 | ("fenton", "0.75"): 1.0/5.7, 41 | ("fenton", "2"): 1.0/8.5, 42 | ("fenton", "4.25"): 1.0/11.4, 43 | ("fenton", "7.4"): 1.0/14.3, 44 | ("pmstokes", "0.135"): 1.0/3.0, 45 | ("pmstokes", "0.75"): 1.0/5.7, 46 | ("pmstokes", "2"): 1.0/8.5, 47 | ("pmstokes", "4.25"): 1.0/11.4, 48 | ("pmstokes", "7.4"): 1.0/14.3, 49 | } 50 | 51 | # Group files by tracker 52 | tracker_groups = {} 53 | for f in files: 54 | m = pattern.search(os.path.basename(f)) 55 | if not m: 56 | print(f"Skipping unrecognized filename: {f}") 57 | continue 58 | tracker = m.group("tracker") 59 | tracker_groups.setdefault(tracker, []).append(f) 60 | 61 | # Plot for each tracker 62 | for tracker, tracker_files in tracker_groups.items(): 63 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(14, 12), sharex=True) 64 | 65 | # Group files by wave type 66 | wave_grouped = {} 67 | for f in tracker_files: 68 | m = pattern.search(os.path.basename(f)) 69 | wave = m.group("wave") 70 | if wave == "gerstner" : 71 | continue 72 | wave_grouped.setdefault(wave, []).append(f) 73 | 74 | for wave, files_in_wave in wave_grouped.items(): 75 | cmap = plt.get_cmap(wave_colors.get(wave, "gray")) 76 | n_files = len(files_in_wave) 77 | 78 | for idx, f in enumerate(sorted(files_in_wave)): 79 | m = pattern.search(os.path.basename(f)) 80 | height = m.group("height").rstrip('0').rstrip('.') # normalize 81 | 82 | df = pd.read_csv(f) 83 | if "regularity" not in df.columns or \ 84 | "significant_wave_height" not in df.columns or \ 85 | "disp_freq_hz" not in df.columns: 86 | print(f"Skipping {f} (missing required columns)") 87 | continue 88 | 89 | color = cmap(0.3 + 0.7 * idx / max(1, n_files - 1)) 90 | label = f"{wave}-h{height}" 91 | 92 | # Top: Regularity 93 | ax1.plot(df["time"], df["regularity"], label=label, alpha=0.8, color=color) 94 | 95 | # Middle: Wave Height Envelope 96 | ax2.plot(df["time"], df["significant_wave_height"], label=label, alpha=0.8, color=color) 97 | 98 | # Bottom: Displacement Frequency 99 | ax3.plot(df["time"], df["disp_freq_hz"], label=label, alpha=0.8, color=color) 100 | 101 | # Add target frequency line 102 | target_freq = wave_target_freq.get((wave, height)) 103 | if target_freq: 104 | ax3.hlines(target_freq, xmin=df["time"].iloc[0], xmax=df["time"].iloc[-1], 105 | colors=color, linestyles="dashed", alpha=0.5) 106 | 107 | # Formatting 108 | ax1.set_ylabel("Regularity score (R)") 109 | ax1.set_title(f"Sea State Regularity, Height Envelope & Disp. Freq — {tracker} tracker") 110 | ax1.grid(True, linestyle="--", alpha=0.5) 111 | ax1.legend(fontsize=8, ncol=3) 112 | 113 | ax2.set_ylabel("Wave Height Envelope [m]") 114 | ax2.grid(True, linestyle="--", alpha=0.5) 115 | 116 | ax3.set_xlabel("Time [s]") 117 | ax3.set_ylabel("Displacement Frequency [Hz]") 118 | ax3.grid(True, linestyle="--", alpha=0.5) 119 | 120 | plt.tight_layout() 121 | plt.show() 122 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/GPS_to_INS.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | // WGS84 constants 6 | constexpr double WGS84_A = 6378137.0; // semi-major axis [m] 7 | constexpr double WGS84_F = 1.0/298.257223563; 8 | constexpr double DEG2RAD = M_PI / 180.0; 9 | constexpr double KNOTS2MPS = 0.514444; 10 | 11 | class GPS_to_INS { 12 | public: 13 | using Vector3 = Eigen::Matrix; 14 | using Matrix3 = Eigen::Matrix; 15 | 16 | struct Result { 17 | Vector3 measurement = Vector3::Zero(); // NED measurement (pos or vel) 18 | Matrix3 covariance = Matrix3::Identity(); 19 | bool valid = false; // accepted by quality checks 20 | }; 21 | 22 | // Constructor: set reference origin (deg, deg, m) 23 | GPS_to_INS(double lat0_deg, double lon0_deg, double alt0_m) 24 | { 25 | lat0_rad_ = lat0_deg * DEG2RAD; 26 | lon0_rad_ = lon0_deg * DEG2RAD; 27 | alt0_m_ = alt0_m; 28 | 29 | // Rotation ECEF->NED 30 | double sLat = std::sin(lat0_rad_), cLat = std::cos(lat0_rad_); 31 | double sLon = std::sin(lon0_rad_), cLon = std::cos(lon0_rad_); 32 | R_ecef2ned_ << 33 | -sLat*cLon, -sLat*sLon, cLat, 34 | -sLon, cLon, 0.0, 35 | -cLat*cLon, -cLat*sLon, -sLat; 36 | 37 | ref_ecef_ = lla_to_ecef(lat0_rad_, lon0_rad_, alt0_m_); 38 | } 39 | 40 | // === Build position measurement (lat/lon/alt → NED + covariance) === 41 | Result build_position_measurement(double lat_deg, double lon_deg, double alt_m, 42 | int fixType, double hdop, double vdop, 43 | double max_hdop=5.0, double max_vdop=10.0) const 44 | { 45 | Result out; 46 | if (fixType <= 0) return out; // no fix 47 | if (hdop > max_hdop || vdop > max_vdop) return out; 48 | 49 | out.measurement = lla_to_ned(lat_deg, lon_deg, alt_m); 50 | 51 | // Covariance from DOPs 52 | out.covariance = Matrix3::Zero(); 53 | double sigma_h = hdop * 1.5; // base 1.5 m 54 | double sigma_v = vdop * 3.0; // base 3.0 m 55 | out.covariance(0,0) = sigma_h*sigma_h; 56 | out.covariance(1,1) = sigma_h*sigma_h; 57 | out.covariance(2,2) = sigma_v*sigma_v; 58 | 59 | out.valid = true; 60 | return out; 61 | } 62 | 63 | // === Build velocity measurement (COG/SOG → NED + covariance) === 64 | Result build_velocity_measurement(double cog_deg, double sog, bool sog_in_knots, 65 | double climb_rate_mps, 66 | int fixType, double sAcc=0.2, 67 | double max_sAcc=2.0) const 68 | { 69 | Result out; 70 | if (fixType <= 0) return out; 71 | if (sAcc > max_sAcc) return out; 72 | 73 | double sog_mps = sog_in_knots ? sog*KNOTS2MPS : sog; 74 | double cog_rad = cog_deg * DEG2RAD; 75 | double vN = sog_mps * std::cos(cog_rad); 76 | double vE = sog_mps * std::sin(cog_rad); 77 | double vD = -climb_rate_mps; // Down positive 78 | out.measurement = Vector3(vN, vE, vD); 79 | 80 | // Covariance from reported sAcc 81 | out.covariance = Matrix3::Zero(); 82 | out.covariance(0,0) = sAcc*sAcc; 83 | out.covariance(1,1) = sAcc*sAcc; 84 | out.covariance(2,2) = std::max(0.5, sAcc*2.0) * std::max(0.5, sAcc*2.0); 85 | 86 | out.valid = true; 87 | return out; 88 | } 89 | 90 | private: 91 | double lat0_rad_, lon0_rad_, alt0_m_; 92 | Vector3 ref_ecef_; 93 | Eigen::Matrix R_ecef2ned_; 94 | 95 | Vector3 lla_to_ned(double lat_deg, double lon_deg, double alt_m) const { 96 | Vector3 ecef = lla_to_ecef(lat_deg*DEG2RAD, lon_deg*DEG2RAD, alt_m); 97 | Vector3 d_ecef = ecef - ref_ecef_; 98 | return R_ecef2ned_ * d_ecef; 99 | } 100 | 101 | static Vector3 lla_to_ecef(double lat_rad, double lon_rad, double alt_m) { 102 | double a = WGS84_A; 103 | double f = WGS84_F; 104 | double e2 = f*(2-f); 105 | double sLat = std::sin(lat_rad), cLat = std::cos(lat_rad); 106 | double sLon = std::sin(lon_rad), cLon = std::cos(lon_rad); 107 | 108 | double N = a / std::sqrt(1 - e2*sLat*sLat); 109 | double x = (N+alt_m) * cLat * cLon; 110 | double y = (N+alt_m) * cLat * sLon; 111 | double z = (N*(1-e2)+alt_m) * sLat; 112 | return Vector3(x,y,z); 113 | } 114 | }; 115 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/TimeAwareSpikeFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef TIME_AWARE_SPIKE_FILTER_H 2 | #define TIME_AWARE_SPIKE_FILTER_H 3 | 4 | /** 5 | * Copyright 2025, Mikhail Grushinskiy 6 | */ 7 | 8 | class TimeAwareSpikeFilter { 9 | private: 10 | struct Sample { 11 | float value; 12 | float deltaTime; // Time since previous sample 13 | }; 14 | 15 | Sample *window; // Circular buffer for previous values and time deltas 16 | int windowSize; // Size of the moving window 17 | int currentIndex; // Current position in the circular buffer 18 | float threshold; // Derivative threshold for spike detection 19 | bool initialized; // Flag to indicate if filter is initialized 20 | 21 | float *derivatives; 22 | float *values; 23 | float *temp; 24 | 25 | public: 26 | /** 27 | @brief Construct a new Spike Filter object 28 | @param size Size of the moving window (recommend 3-5) 29 | @param thr Threshold for spike detection (tune based on your signal) 30 | */ 31 | TimeAwareSpikeFilter(int size, float thr) : windowSize(size), threshold(thr), initialized(false) { 32 | window = new Sample[windowSize]; 33 | values = new float[windowSize]; 34 | derivatives = new float[windowSize]; 35 | temp = new float[windowSize]; 36 | currentIndex = 0; 37 | } 38 | 39 | ~TimeAwareSpikeFilter() { 40 | delete[] temp; 41 | delete[] derivatives; 42 | delete[] values; 43 | delete[] window; 44 | } 45 | 46 | /** 47 | @brief Filtering when you already have the delta time 48 | @param input Raw input value 49 | @param deltaT Time since last sample 50 | @return Filtered output value 51 | */ 52 | float filterWithDelta(float input, float deltaT) { 53 | if (!initialized) { 54 | // Initialize the window with the first value 55 | for (int i = 0; i < windowSize; i++) { 56 | window[i].value = input; 57 | window[i].deltaTime = deltaT; // Initialize 58 | } 59 | initialized = true; 60 | return input; 61 | } 62 | 63 | // Store the new value and time delta 64 | window[currentIndex].value = input; 65 | window[currentIndex].deltaTime = deltaT; 66 | currentIndex = (currentIndex + 1) % windowSize; 67 | 68 | // Calculate time-weighted derivatives 69 | for (int i = 0; i < windowSize; i++) { 70 | int currentIdx = (currentIndex + i) % windowSize; 71 | int prevIdx = (currentIndex + i - 1 + windowSize) % windowSize; 72 | 73 | float dt = window[currentIdx].deltaTime; 74 | 75 | derivatives[i] = (window[currentIdx].value - window[prevIdx].value) / dt; 76 | } 77 | 78 | // Find the median derivative (more robust than average) 79 | float medianDerivative = computeMedian(derivatives, windowSize); 80 | 81 | // Check if current derivative is a spike 82 | int prevIdx = (currentIndex - 1 + windowSize) % windowSize; 83 | float currentDeltaT = window[currentIndex].deltaTime; 84 | float currentDerivative = (window[currentIndex].value - window[prevIdx].value) / currentDeltaT; 85 | 86 | if (fabs(currentDerivative - medianDerivative) > threshold) { 87 | // Spike detected - replace with median of window values 88 | for (int i = 0; i < windowSize; i++) { 89 | values[i] = window[i].value; 90 | } 91 | float medianValue = computeMedian(values, windowSize); 92 | return medianValue; 93 | } 94 | 95 | // No spike - return the original value 96 | return input; 97 | } 98 | 99 | private: 100 | /** 101 | @brief Computes the median of an array 102 | @param arr Input array 103 | @param n Size of array 104 | @return Median value 105 | */ 106 | float computeMedian(float *arr, int n) { 107 | // Create a copy to avoid modifying original 108 | for (int i = 0; i < n; i++) { 109 | temp[i] = arr[i]; 110 | } 111 | 112 | // Sort the array (bubble sort for simplicity) 113 | for (int i = 0; i < n - 1; i++) { 114 | for (int j = 0; j < n - i - 1; j++) { 115 | if (temp[j] > temp[j + 1]) { 116 | float swap = temp[j]; 117 | temp[j] = temp[j + 1]; 118 | temp[j + 1] = swap; 119 | } 120 | } 121 | } 122 | 123 | // Return median 124 | if (n % 2 == 1) { 125 | return temp[n / 2]; 126 | } else { 127 | return (temp[n / 2 - 1] + temp[n / 2]) / 2.0f; 128 | } 129 | } 130 | }; 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/qmekf_plots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import glob 3 | import os 4 | import re 5 | import pandas as pd 6 | import matplotlib as mpl 7 | import matplotlib.pyplot as plt 8 | 9 | # === Matplotlib PGF/LaTeX config === 10 | mpl.use("pgf") 11 | plt.rcParams.update({ 12 | "pgf.texsystem": "xelatex", 13 | "font.family": "serif", 14 | "text.usetex": True, 15 | "pgf.rcfonts": False, 16 | "pgf.preamble": "\n".join([ 17 | r"\usepackage{fontspec}", 18 | r"\usepackage{unicode-math}", 19 | r"\usepackage{amsmath}", 20 | r"\setmainfont{DejaVu Serif}", 21 | r"\setmathfont{Latin Modern Math}", 22 | r"\providecommand{\mathdefault}[1]{#1}" 23 | ]) 24 | }) 25 | 26 | # === Config === 27 | DATA_DIR = "./" # Directory with *_kalman.csv files 28 | SAMPLE_RATE_HZ = 240 # Simulator sample rate 29 | SKIP_TIME_S = 840.0 # Skip first n seconds (warmup) 30 | PLOT_TIME_S = 60.0 # Plot next m seconds 31 | MAX_TIME_S = SKIP_TIME_S + PLOT_TIME_S 32 | MAX_ROWS = int(SAMPLE_RATE_HZ * MAX_TIME_S) 33 | 34 | # === Groups we care about (included heights in meters) === 35 | height_groups = { 36 | "low": 0.27, 37 | "medium": 1.50, 38 | "high": 8.50, 39 | } 40 | 41 | # === Allowed wave types === 42 | ALLOWED_WAVES = {"jonswap", "pmstokes"} 43 | 44 | # === Regex to extract wave type and height from filename === 45 | pattern = re.compile( 46 | r".*?_(?P[a-zA-Z0-9]+)_H(?P[-0-9\.]+).*?_kalman\.csv$" 47 | ) 48 | 49 | # === Escape helper for LaTeX === 50 | def latex_safe(s: str) -> str: 51 | return s.replace("_", r"\_") 52 | 53 | # === Find all *_kalman.csv files === 54 | files = glob.glob(os.path.join(DATA_DIR, "*_kalman.csv")) 55 | if not files: 56 | print("No *_kalman.csv files found in", DATA_DIR) 57 | exit() 58 | 59 | generated = [] # store (wave_type, group_name, pgf_filename) 60 | 61 | # === Process each file === 62 | for fname in files: 63 | basename = os.path.basename(fname) 64 | m = pattern.match(basename) 65 | if not m: 66 | print(f"Skipping {fname} (could not parse)") 67 | continue 68 | 69 | wave_type = m.group("wave").lower() 70 | if wave_type not in ALLOWED_WAVES: 71 | print(f"Skipping {fname} (wave={wave_type} not included)") 72 | continue 73 | 74 | try: 75 | height_val = float(m.group("height")) 76 | except (TypeError, ValueError): 77 | print(f"Skipping {fname} (invalid height)") 78 | continue 79 | 80 | # Map height to group 81 | group_name = None 82 | for name, h in height_groups.items(): 83 | if abs(height_val - h) < 1e-6: 84 | group_name = name 85 | break 86 | if group_name is None: 87 | print(f"Skipping {fname} (height {height_val} m not in groups)") 88 | continue 89 | 90 | # Build output base name (safe for filesystem) 91 | outbase = f"qmekf_{wave_type}_{group_name}" 92 | outbase = re.sub(r"[^A-Za-z0-9_\-]", "_", outbase) 93 | outbase = os.path.join(DATA_DIR, outbase) 94 | 95 | print(f"Plotting {fname} → {outbase} ...") 96 | 97 | # Load limited rows 98 | df = pd.read_csv(fname, nrows=MAX_ROWS) 99 | df = df[(df["time"] >= SKIP_TIME_S) & (df["time"] <= MAX_TIME_S)].reset_index(drop=True) 100 | time = df["time"] 101 | 102 | # Reference vs estimated Euler angles 103 | angles = [ 104 | ("roll_ref", "roll_est", "Roll (deg)"), 105 | ("pitch_ref", "pitch_est", "Pitch (deg)"), 106 | ("yaw_ref", "yaw_est", "Yaw (deg)"), 107 | ] 108 | 109 | fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True) 110 | 111 | # Escape underscores in the title 112 | fig.suptitle(latex_safe(basename)) 113 | 114 | for ax, (ref_col, est_col, label) in zip(axes, angles): 115 | ax.plot(time, df[ref_col], label=latex_safe("Reference"), linewidth=1.5) 116 | ax.plot(time, df[est_col], label=latex_safe("Estimated"), 117 | linewidth=1.0, linestyle="--") 118 | ax.set_ylabel(latex_safe(label)) 119 | ax.grid(True) 120 | ax.legend(loc="upper right") 121 | 122 | axes[-1].set_xlabel(latex_safe("Time (s)")) 123 | plt.tight_layout(rect=[0, 0.03, 1, 0.95]) 124 | 125 | # Save to PGF and SVG 126 | pgf_out = f"{outbase}.pgf" 127 | svg_out = f"{outbase}.svg" 128 | plt.savefig(pgf_out, format="pgf", bbox_inches="tight") 129 | plt.savefig(svg_out, format="svg", bbox_inches="tight") 130 | plt.close(fig) 131 | 132 | print(f"Saved {pgf_out} and {svg_out}") 133 | generated.append((wave_type, group_name, os.path.basename(pgf_out))) 134 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/wdir_plots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Rose-only plots for wdir_*.csv, with loud logging and safe backend (Agg) 3 | 4 | import argparse 5 | from pathlib import Path 6 | import glob 7 | import math 8 | import numpy as np 9 | import pandas as pd 10 | import matplotlib 11 | matplotlib.use("Agg") # ensure non-GUI backend so saving works anywhere 12 | import matplotlib.pyplot as plt 13 | import os 14 | from typing import Optional 15 | 16 | def ensure_dir(p: Path): 17 | p.mkdir(parents=True, exist_ok=True) 18 | 19 | def rose_180(theta_deg: np.ndarray, 20 | outpath: Path, 21 | bins: int = 36, 22 | weights: Optional[np.ndarray] = None, 23 | title: str = "Directional rose"): 24 | if theta_deg.size == 0: 25 | print(f" [skip] no angles → {outpath}") 26 | return 27 | theta = np.deg2rad(np.mod(theta_deg, 180.0)) # [0, pi) 28 | counts, edges = np.histogram(theta, bins=bins, range=(0.0, math.pi), weights=weights) 29 | centers = 0.5 * (edges[:-1] + edges[1:]) 30 | widths = (edges[1] - edges[0]) * np.ones_like(centers) 31 | 32 | fig, ax = plt.subplots(subplot_kw={"projection": "polar"}) 33 | ax.set_theta_zero_location("N") 34 | ax.set_theta_direction(-1) 35 | ax.bar(centers, counts, width=widths, align="center") 36 | ax.set_title(title) 37 | fig.tight_layout() 38 | ensure_dir(outpath.parent) 39 | fig.savefig(outpath, dpi=150) 40 | plt.close(fig) 41 | print(f" [write] {outpath}") 42 | 43 | def process_file(csv_path: Path, outroot: Path, bins: int): 44 | outdir = outroot / csv_path.stem 45 | ensure_dir(outdir) 46 | print(f"[rose] {csv_path} → {outdir}") 47 | 48 | df = pd.read_csv(csv_path) 49 | 50 | if "dir_deg" not in df.columns: 51 | print(" [skip] missing 'dir_deg'") 52 | return 53 | 54 | theta_all = df["dir_deg"].dropna().to_numpy() 55 | if theta_all.size == 0: 56 | print(" [skip] 'dir_deg' is empty") 57 | return 58 | 59 | # 1) plain counts 60 | rose_180(theta_all, outdir / "dir_rose.png", bins=bins, 61 | title=f"{csv_path.name} — dir_rose (counts)") 62 | 63 | # 2) confidence-weighted (if present) 64 | if "dir_conf" in df.columns: 65 | mask = df["dir_deg"].notna().to_numpy() 66 | w = df["dir_conf"].fillna(0.0).to_numpy() 67 | if w.shape[0] == df.shape[0]: 68 | rose_180(theta_all, outdir / "dir_rose_weighted_conf.png", 69 | bins=bins, weights=w[mask], 70 | title=f"{csv_path.name} — dir_rose (weighted by dir_conf)") 71 | 72 | # 3) split by sign if present 73 | if "dir_sign_num" in df.columns: 74 | sign = df["dir_sign_num"] 75 | toward = df.loc[(sign == 1) & df["dir_deg"].notna(), "dir_deg"].to_numpy() 76 | if toward.size: 77 | rose_180(toward, outdir / "dir_rose_toward.png", bins=bins, 78 | title=f"{csv_path.name} — dir_rose (TOWARD)") 79 | away = df.loc[(sign == -1) & df["dir_deg"].notna(), "dir_deg"].to_numpy() 80 | if away.size: 81 | rose_180(away, outdir / "dir_rose_away.png", bins=bins, 82 | title=f"{csv_path.name} — dir_rose (AWAY)") 83 | 84 | def main(): 85 | ap = argparse.ArgumentParser() 86 | ap.add_argument("--glob", default="wdir_*.csv", 87 | help="glob for input CSV files (use ** for recursion)") 88 | ap.add_argument("--recursive", action="store_true", 89 | help="enable recursive globbing") 90 | ap.add_argument("--outdir", default="wdir_plots", 91 | help="root output directory") 92 | ap.add_argument("--bins", type=int, default=36, 93 | help="number of bins on [0,180)") 94 | args = ap.parse_args() 95 | 96 | cwd = Path.cwd() 97 | print(f"[info] cwd = {cwd}") 98 | print(f"[info] searching: {args.glob} (recursive={args.recursive})") 99 | 100 | files = sorted(glob.glob(args.glob, recursive=args.recursive)) 101 | print(f"[info] matched {len(files)} file(s)") 102 | for f in files: 103 | print(f" - {f}") 104 | 105 | if not files: 106 | print("No files matched. Tips:") 107 | print(" • Run from the folder that contains your wdir_*.csv") 108 | print(r" • Or pass a pattern like --glob .\**\wdir_*.csv --recursive") 109 | print(r" • Or give an absolute pattern, e.g. --glob D:\path\to\wdir_*.csv") 110 | return 111 | 112 | outroot = Path(args.outdir) 113 | for f in files: 114 | process_file(Path(f), outroot, bins=args.bins) 115 | 116 | print(f"[done] outputs are under: {outroot.resolve()}") 117 | 118 | if __name__ == "__main__": 119 | main() 120 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-dnoise.tex-part: -------------------------------------------------------------------------------- 1 | \section{Summary of Discrete Noise Blocks} 2 | 3 | The process covariance matrix~$\mathbf{Q}$ aggregates all sources of 4 | stochastic excitation acting on the state between two sampling instants. 5 | It preserves the block structure of the transition matrix~$\Phi$ and is 6 | assembled from three principal contributions: 7 | \[ 8 | \mathbf{Q} 9 | = 10 | \mathrm{diag} 11 | \left( 12 | \mathbf{Q}_{AA},\; 13 | \mathbf{Q}_{LL},\; 14 | \mathbf{Q}_{b_a} 15 | \right) 16 | + 17 | \text{(cross terms, if enabled)}. 18 | \] 19 | 20 | \subsection{Attitude and Gyro-Bias Noise \texorpdfstring{$\mathbf{Q}_{AA}$}{Q\_AA}} 21 | 22 | The rotational subsystem is excited by gyroscope measurement noise and 23 | gyro-bias random walk. In continuous time, these are modeled as 24 | \[ 25 | \dot{\delta\boldsymbol{\theta}} = 26 | -[\boldsymbol{\omega}]_\times \delta\boldsymbol{\theta} 27 | - \delta\mathbf{b}_g + \boldsymbol{\nu}_g, \qquad 28 | \dot{\mathbf{b}}_g = \boldsymbol{\xi}_g, 29 | \] 30 | with white-noise processes 31 | $\boldsymbol{\nu}_g \sim \mathcal{N}(0,\Sigma_g)$ and 32 | $\boldsymbol{\xi}_g \sim \mathcal{N}(0,q_b \mathbf{I})$. 33 | Discretization over step~$h$ gives 34 | \begin{equation} 35 | \mathbf{Q}_{AA} 36 | = 37 | \begin{bmatrix} 38 | \Sigma_g h & 0\\ 39 | 0 & q_b h 40 | \end{bmatrix}, 41 | \end{equation} 42 | which directly populates the upper-left block of~$\mathbf{Q}$. 43 | This term governs attitude diffusion during free propagation and is 44 | typically small compared with translational noise. 45 | 46 | \subsection{Linear Translational Noise \texorpdfstring{$\mathbf{Q}_{LL}$}{Q\_LL}} 47 | 48 | The OU acceleration process introduces temporally correlated forcing to 49 | the translational states. 50 | For a single spatial axis with stationary variance 51 | $\sigma_{a_w}^2$ and correlation time~$\tau$, the equivalent discrete 52 | noise covariance is computed as 53 | \begin{equation} 54 | \mathbf{Q}_d^{(1)} 55 | = \frac{2\sigma_{a_w}^2}{\tau} 56 | \int_0^h \!\!\int_0^h e^{-|t-s|/\tau} 57 | \mathbf{f}(t)\mathbf{f}^\top(s)\,dt\,ds, 58 | \end{equation} 59 | where $\mathbf{f}(t)=[1,\,t,\,t^2/2!,\,a(t)]^\top$ corresponds to the 60 | OU-driven chain $[v,p,S,a_w]$. 61 | The analytic expression of~$\mathbf{Q}_d^{(1)}$ is evaluated using the 62 | functions derived in the \emph{OU discretization primitives}, switching 63 | to a Maclaurin expansion for~$h/\tau<10^{-3}$. 64 | 65 | For the three-dimensional vector process with stationary covariance 66 | $\Sigma_{a_w}$, the full block is constructed as 67 | \[ 68 | \mathbf{Q}_{LL} 69 | = \Sigma_{a_w}^{1/2} \otimes \mathbf{Q}_d^{(1)} 70 | (\Sigma_{a_w}^{1/2})^\top, 71 | \] 72 | followed by explicit symmetrization and eigenvalue flooring to ensure 73 | positive semidefiniteness: 74 | \[ 75 | \mathbf{Q}_{LL} \leftarrow 76 | \tfrac{1}{2}(\mathbf{Q}_{LL} + \mathbf{Q}_{LL}^\top), 77 | \qquad 78 | \lambda_i(\mathbf{Q}_{LL}) \ge \varepsilon. 79 | \] 80 | This guarantees stable propagation even when cross-axis correlations or 81 | very small~$\tau$ produce ill-conditioned matrices. 82 | 83 | \subsection{Accelerometer-Bias Noise \texorpdfstring{$\mathbf{Q}_{b_a}$}{Q\_b\_a}} 84 | 85 | The accelerometer bias evolves as an independent random walk, 86 | \[ 87 | \dot{\mathbf{b}}_a = \boldsymbol{\xi}_a, \qquad 88 | \boldsymbol{\xi}_a \sim \mathcal{N}(0,\,\Sigma_{b_a}), 89 | \] 90 | with spectral density $\Sigma_{b_a}$ determined by the user-specified 91 | random-walk standard deviations. Its discrete form is 92 | \[ 93 | \mathbf{Q}_{b_a} = \Sigma_{b_a} h. 94 | \] 95 | This term directly updates the bias covariance block at each prediction 96 | step and remains uncoupled from the other subsystems. 97 | 98 | \subsection{Assembled Discrete Propagation} 99 | 100 | Combining all components, the discrete prediction step for mean and 101 | covariance can be written as 102 | \begin{align} 103 | \hat{\mathbf{x}}_{k+1|k} &= \Phi\,\hat{\mathbf{x}}_{k|k},\\ 104 | \mathbf{P}_{k+1|k} 105 | &= \Phi\,\mathbf{P}_{k|k}\,\Phi^\top + \mathbf{Q}, 106 | \end{align} 107 | with explicit block substitutions: 108 | \begin{align} 109 | \mathbf{P}_{AA}^{+} 110 | &= \Phi_{AA}\mathbf{P}_{AA}\Phi_{AA}^\top + \mathbf{Q}_{AA},\\ 111 | \mathbf{P}_{LL}^{+} 112 | &= \Phi_{LL}\mathbf{P}_{LL}\Phi_{LL}^\top + \mathbf{Q}_{LL},\\ 113 | \mathbf{P}_{b_a b_a}^{+} 114 | &= \mathbf{P}_{b_a b_a} + \mathbf{Q}_{b_a},\\ 115 | \mathbf{P}_{AL}^{+} 116 | &= \Phi_{AA}\mathbf{P}_{AL}\Phi_{LL}^\top,\\ 117 | \mathbf{P}_{A b_a}^{+} 118 | &= \Phi_{AA}\mathbf{P}_{A b_a}, \qquad 119 | \mathbf{P}_{L b_a}^{+} = \Phi_{LL}\mathbf{P}_{L b_a}. 120 | \end{align} 121 | The final covariance is symmetrized, 122 | $\mathbf{P}\!\leftarrow\!\tfrac{1}{2}(\mathbf{P}+\mathbf{P}^\top)$, 123 | ensuring that rounding errors never violate 124 | $\mathbf{P}\succeq 0$. This completes the discrete process model used 125 | throughout the propagation cycle. 126 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/reg_spectra_plots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import glob 3 | import os 4 | import re 5 | import pandas as pd 6 | import matplotlib.pyplot as plt 7 | import matplotlib as mpl 8 | 9 | # === Matplotlib PGF/LaTeX config === 10 | mpl.use("pgf") 11 | plt.rcParams.update({ 12 | "pgf.texsystem": "xelatex", 13 | "font.family": "serif", 14 | "text.usetex": True, 15 | "pgf.rcfonts": False, 16 | "pgf.preamble": "\n".join([ 17 | r"\usepackage{fontspec}", 18 | r"\usepackage{unicode-math}", 19 | r"\usepackage{amsmath}", 20 | r"\setmainfont{DejaVu Serif}", 21 | r"\setmathfont{Latin Modern Math}", 22 | r"\providecommand{\mathdefault}[1]{#1}" 23 | ]) 24 | }) 25 | 26 | # === Height classification === 27 | height_groups = {"low": 0.27, "medium": 1.50, "high": 8.50} 28 | def classify_height(h): 29 | diffs = {lvl: abs(h - v) for lvl, v in height_groups.items()} 30 | return min(diffs, key=diffs.get) 31 | 32 | # === Filename pattern === 33 | pattern = re.compile( 34 | r"reg_spectrum_" 35 | r"(?P[^_]+)_" # tracker (aranovskiy, kalmanf, zerocross) 36 | r"(?P[A-Za-z0-9]+)_" # wave type (jonswap, pmstokes, etc.) 37 | r"H(?P[-0-9\.]+)" # H0.27 38 | r"(?:_L(?P[-0-9\.]+))?" # optional L14.047 39 | r"(?:_A(?P[-0-9\.]+))?" # optional A25 40 | r"(?:_P(?P[-0-9\.]+))?" # optional P60 41 | r"_N(?P[-0-9\.]+)" # N0.080 42 | r"_B(?P[-0-9\.]+)" # B0.100 43 | r"\.csv" 44 | ) 45 | 46 | # === Load all files and group by (wave, height) === 47 | groups = {} 48 | for f in sorted(glob.glob("reg_spectrum_*.csv")): 49 | m = pattern.search(os.path.basename(f)) 50 | if not m: 51 | print(f"Skipping unrecognized: {f}") 52 | continue 53 | wave = m.group("wave") 54 | height = float(m.group("height")) 55 | tracker = m.group("tracker") 56 | key = (wave, height) 57 | groups.setdefault(key, []).append((tracker, f)) 58 | 59 | if not groups: 60 | print("No recognized reg_spectrum_*.csv files found.") 61 | exit(0) 62 | 63 | # === Plot each wave × height group with all trackers === 64 | for (wave, height), tracker_files in groups.items(): 65 | level = classify_height(height) 66 | height_str = f"H{height:.2f}".rstrip('0').rstrip('.') 67 | print(f"\nPlotting {wave} {level} sea ({height_str} m) with {len(tracker_files)} trackers") 68 | 69 | fig, axes = plt.subplots(3, 1, figsize=(6.5, 8.5), sharex=True) 70 | axes = axes.flatten() 71 | 72 | for tracker, f in tracker_files: 73 | df = pd.read_csv(f, comment="#") 74 | if "freq_hz" not in df.columns: 75 | print(f" skipping {f} (no freq_hz)") 76 | continue 77 | 78 | label = tracker.capitalize() 79 | lw = 1.8 80 | 81 | # 1. Amplitude spectra 82 | axes[0].semilogx(df["freq_hz"], df["A_eta_est"], label=f"{label} est", lw=lw) 83 | if "A_eta_ref" in df.columns and tracker_files.index((tracker, f)) == 0: 84 | axes[0].semilogx(df["freq_hz"], df["A_eta_ref"], "--", color="gray", label="Reference", lw=1.2) 85 | 86 | # 2. Energy density 87 | axes[1].semilogx(df["freq_hz"], df["E_eta_est"], label=f"{label} est", lw=lw) 88 | if "E_eta_ref" in df.columns and tracker_files.index((tracker, f)) == 0: 89 | axes[1].semilogx(df["freq_hz"], df["E_eta_ref"], "--", color="gray", label="Reference", lw=1.2) 90 | 91 | # 3. Cumulative variance 92 | if {"CumVar_est", "CumVar_ref"}.issubset(df.columns): 93 | est_norm = df["CumVar_est"] / df["CumVar_est"].iloc[-1] 94 | ref_norm = df["CumVar_ref"] / df["CumVar_ref"].iloc[-1] 95 | axes[2].semilogx(df["freq_hz"], est_norm, label=f"{label} est", lw=lw) 96 | if tracker_files.index((tracker, f)) == 0: 97 | axes[2].semilogx(df["freq_hz"], ref_norm, "--", color="gray", label="Reference", lw=1.2) 98 | 99 | # === Axis formatting === 100 | axes[0].set_ylabel(r"$A_\eta(f)$ [m]") 101 | axes[1].set_ylabel(r"$E_\eta(f)=fS_\eta(f)$ [m$^2$]") 102 | axes[2].set_ylabel("Cumulative variance fraction") 103 | axes[2].set_xlabel("Frequency [Hz]") 104 | for ax in axes: 105 | ax.grid(True, which="both", lw=0.3, ls=":") 106 | ax.legend(fontsize=7) 107 | ax.set_xlim(left=0.02, right=2.0) 108 | 109 | fig.suptitle(f"{wave.upper()} — {level.capitalize()} sea ($H_s={height:.2f}$ m)", fontsize=11, y=0.97) 110 | fig.tight_layout(rect=[0, 0, 1, 0.96]) 111 | 112 | out_pgf = f"reg_spectrum_plot_{wave}_{level}.pgf" 113 | plt.savefig(out_pgf, bbox_inches="tight") 114 | print(f" Saved → {out_pgf}") 115 | 116 | plt.close(fig) 117 | 118 | print("\nAll combined reg_spectrum plots generated successfully.") 119 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/plots/freq_track_plots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import re 4 | import glob 5 | import pandas as pd 6 | import matplotlib.pyplot as plt 7 | import matplotlib as mpl 8 | 9 | # === Matplotlib PGF/LaTeX config === 10 | mpl.use("pgf") 11 | plt.rcParams.update({ 12 | "pgf.texsystem": "xelatex", 13 | "font.family": "serif", 14 | "text.usetex": True, 15 | "pgf.rcfonts": False, 16 | "pgf.preamble": "\n".join([ 17 | r"\usepackage{fontspec}", 18 | r"\usepackage{unicode-math}", 19 | r"\usepackage{amsmath}", 20 | r"\setmainfont{DejaVu Serif}", 21 | r"\setmathfont{Latin Modern Math}", 22 | r"\providecommand{\mathdefault}[1]{#1}" 23 | ]) 24 | }) 25 | 26 | # === Folder with tracker CSV files === 27 | DATA_FOLDER = "./" 28 | 29 | # Regex for naming convention with N and B in input CSV 30 | FNAME_RE = re.compile( 31 | r'^freq_track_(?Paranovskiy|kalmanf|zerocross)' 32 | r'_(?Pgerstner|jonswap|fenton|pmstokes|cnoidal)' 33 | r'_H(?P[0-9.]+)_L(?P[0-9.]+)_A(?P[-0-9.]+)_P(?P

[-0-9.]+)' 34 | r'_N(?P[0-9.]+)_B(?P[0-9.]+)\.csv$' 35 | ) 36 | 37 | # Sampling config 38 | SAMPLE_RATE_HZ = 240 39 | MAX_TIME_S = 300.0 40 | MAX_ROWS = int(SAMPLE_RATE_HZ * MAX_TIME_S) 41 | 42 | # === Load all tracker CSV files === 43 | def load_all_data(folder): 44 | data = [] 45 | for filepath in glob.glob(os.path.join(folder, "freq_track_*.csv")): 46 | fname = os.path.basename(filepath) 47 | m = FNAME_RE.match(fname) 48 | if not m: 49 | continue 50 | tracker = m.group("tracker") 51 | wave = m.group("wave") 52 | H = float(m.group("H")) 53 | L = float(m.group("L")) 54 | A = float(m.group("A")) 55 | P = float(m.group("P")) 56 | N = float(m.group("N")) 57 | B = float(m.group("B")) 58 | 59 | # Limit to first 300 seconds (≈72k samples) 60 | df = pd.read_csv(filepath, nrows=MAX_ROWS) 61 | df['tracker'] = tracker 62 | df['wave'] = wave 63 | df['H'] = H 64 | df['L'] = L 65 | df['A'] = A 66 | df['P'] = P 67 | df['N'] = N 68 | df['B'] = B 69 | data.append(df) 70 | 71 | if not data: 72 | raise RuntimeError("No matching tracker CSV files found in folder") 73 | return pd.concat(data, ignore_index=True) 74 | 75 | # === Save function (PGF, SVG) === 76 | def save_all(fig, base): 77 | fig.savefig(f"{base}.pgf", bbox_inches="tight") 78 | fig.savefig(f"{base}.svg", bbox_inches="tight", dpi=150) 79 | print(f" saved {base}.pgf/.svg") 80 | 81 | # === Plot for each (wave, H) scenario === 82 | def plot_scenarios(df): 83 | scenarios = df.groupby(['wave', 'H', 'N', 'B']) 84 | for (wave, H, N, B), group in scenarios: 85 | fig, ax = plt.subplots(figsize=(12, 6)) 86 | ax.set_title( 87 | f"Frequency Tracking Comparison\n" 88 | f"Wave: {wave}, H={H:.3f} m, Noise={N:.3f}, Bias={B:.3f}" 89 | ) 90 | ax.set_xlabel("Time (s)") 91 | ax.set_ylabel("Frequency (Hz)") 92 | 93 | for tracker in group['tracker'].unique(): 94 | subset = group[group['tracker'] == tracker] 95 | ax.plot(subset['time'], subset['est_freq'], label=f"{tracker} raw") 96 | ax.plot(subset['time'], subset['smooth_freq'], linestyle='--', label=f"{tracker} smooth") 97 | 98 | ax.legend() 99 | ax.grid(True) 100 | plt.tight_layout() 101 | 102 | base = f"freqtrack_{wave}_H{H:.3f}" 103 | save_all(fig, base) 104 | plt.close(fig) 105 | 106 | # === Optional: plot errors for a specific scenario === 107 | def plot_errors(df, wave, H, N, B): 108 | subset = df[(df['wave'] == wave) & (df['H'] == H) & (df['N'] == N) & (df['B'] == B)] 109 | fig, ax = plt.subplots(figsize=(12, 6)) 110 | ax.set_title( 111 | f"Frequency Tracking Errors\n" 112 | f"Wave: {wave}, H={H:.3f} m, Noise={N:.3f}, Bias={B:.3f}" 113 | ) 114 | ax.set_xlabel("Time (s)") 115 | ax.set_ylabel("Frequency Error (Hz)") 116 | 117 | for tracker in subset['tracker'].unique(): 118 | tr_data = subset[subset['tracker'] == tracker] 119 | ax.plot(tr_data['time'], tr_data['error'], label=f"{tracker} raw error") 120 | ax.plot(tr_data['time'], tr_data['smooth_error'], linestyle='--', label=f"{tracker} smooth error") 121 | 122 | ax.legend() 123 | ax.grid(True) 124 | plt.tight_layout() 125 | 126 | base = f"freqtrack_{wave}_H{H:.3f}_errors" 127 | save_all(fig, base) 128 | plt.close(fig) 129 | 130 | # === Main === 131 | def main(): 132 | df = load_all_data(DATA_FOLDER) 133 | print(f"Loaded {len(df)} rows (first {MAX_TIME_S} seconds) from tracker CSV files.") 134 | plot_scenarios(df) 135 | # Example: plot_errors(df, 'pmstokes', 1.500, 0.080, 0.100) 136 | 137 | if __name__ == "__main__": 138 | main() 139 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/WaveFilters.h: -------------------------------------------------------------------------------- 1 | #ifndef WAVE_FILTERS_H 2 | #define WAVE_FILTERS_H 3 | 4 | /* 5 | Copyright 2024-2025, Mikhail Grushinskiy 6 | */ 7 | 8 | #define FREQ_LOWER 0.04f 9 | #define FREQ_UPPER 2.0f 10 | #define FREQ_GUESS 0.3f // frequency guess 11 | 12 | #define ZERO_CROSSINGS_HYSTERESIS 0.04f 13 | #define ZERO_CROSSINGS_PERIODS 1 14 | #define ZERO_CROSSINGS_SCALE 1.0f 15 | #define ZERO_CROSSINGS_DEBOUNCE_TIME 0.12f 16 | #define ZERO_CROSSINGS_STEEPNESS_TIME 0.21f 17 | 18 | #define FREQ_COEF 3.0f 19 | 20 | #define ACCEL_CLAMP 0.5f // fractions of G 21 | 22 | #define ACCEL_SPIKE_FILTER_SIZE 5 23 | #define ACCEL_SPIKE_FILTER_THRESHOLD 1.0f 24 | 25 | enum FrequencyTracker { 26 | Aranovskiy, 27 | Kalm_ANF, 28 | ZeroCrossing 29 | }; 30 | 31 | void init_aranovskiy(AranovskiyFilter* ar_filter); 32 | void init_smoother(KalmanSmootherVars* kalman_smoother); 33 | void init_filters(AranovskiyFilter* ar_filter, KalmanSmootherVars* kalman_smoother); 34 | void init_filters_alt(KalmANF* kalmANF, KalmanSmootherVars* kalman_smoother); 35 | void init_wave_filters(); 36 | 37 | KalmanForWaveBasic wave_filter; 38 | KalmanWaveNumStableAlt wave_alt_filter; 39 | KalmanWaveDirection wave_dir_kalman(FREQ_GUESS); 40 | 41 | template T clamp(T val, T min, T max) { 42 | return (val < min) ? min : (val > max) ? max : val; 43 | } 44 | 45 | int warmup_time_sec(bool use_mahony); 46 | 47 | /* 48 | From experiments QMEKF somehow introduces bigger bias of vertical acceleration. 49 | Longer warm up time is needed to engage Aranovskiy filter. 50 | */ 51 | int warmup_time_sec(bool use_mahony) { 52 | return use_mahony ? 20 : 120; 53 | } 54 | 55 | uint32_t getWindowMicros(double period) { 56 | uint32_t windowMicros = period * 1000000; 57 | return clamp(windowMicros, (uint32_t) 5 * 1000000, (uint32_t) 30 * 1000000); 58 | } 59 | 60 | void init_aranovskiy(AranovskiyFilter* ar_filter) { 61 | /* 62 | Accelerometer bias creates heave bias and Aranovskiy filter gives 63 | lower frequency (i. e. higher period). 64 | Even 2cm bias in heave is too much to affect frequency a lot 65 | */ 66 | double omega_up = (FREQ_GUESS * 2) * (2 * M_PI); // upper angular frequency 67 | double k_gain = 20.0; // Aranovskiy gain. Higher value will give faster convergence, but too high will potentially overflow decimal 68 | double x1_0 = 0.0; 69 | double omega_init = (FREQ_GUESS / 1.5) * 2 * M_PI; 70 | double theta_0 = -(omega_init * omega_init); 71 | double sigma_0 = theta_0; 72 | ar_filter->setParams(omega_up, k_gain); 73 | ar_filter->setState(x1_0, theta_0, sigma_0); 74 | } 75 | 76 | void init_smoother(KalmanSmootherVars* kalman_smoother) { 77 | double process_noise_covariance = 0.25f; 78 | double measurement_uncertainty = 2.0f; 79 | double estimation_uncertainty = 100.0f; 80 | kalman_smoother_init(kalman_smoother, process_noise_covariance, measurement_uncertainty, estimation_uncertainty); 81 | } 82 | 83 | void init_wave_filters() { 84 | wave_filter.initialize(5.0f, 1e-4f, 1e-2f, 1e-5f); 85 | wave_filter.initMeasurementNoise(1e-3f); 86 | //wave_alt_filter.initialize(2.0f, 1e-4f, 1e-2f, 1e+6f, 1e-5f, 0.007f /* typical temperature coefficient for MPU6886 */); 87 | wave_alt_filter.initMeasurementNoise(1e-3f, 1e-2f); 88 | wave_dir_kalman.setMeasurementNoise(0.01f); 89 | wave_dir_kalman.setProcessNoise(1e-6f); 90 | } 91 | 92 | void init_filters(AranovskiyFilter* ar_filter, KalmanSmootherVars* kalman_smoother) { 93 | init_aranovskiy(ar_filter); 94 | init_smoother(kalman_smoother); 95 | init_wave_filters(); 96 | } 97 | 98 | void init_filters_alt(KalmANF* kalmANF, KalmanSmootherVars* kalman_smoother) { 99 | kalmANF->init(0.985f, 1e-6f, 1e+5f, 1.0f, 0.0f, 0.0f, 1.9999f); 100 | init_smoother(kalman_smoother); 101 | init_wave_filters(); 102 | } 103 | 104 | float estimate_freq(FrequencyTracker tracker, AranovskiyFilter* arFilter, KalmANF* kalmANF, 105 | SchmittTriggerFrequencyDetector* freqDetector, float a_noisy, float a_no_spikes, float delta_t, float t) { 106 | float freq = FREQ_GUESS; 107 | if (tracker == Aranovskiy) { 108 | arFilter->update(a_no_spikes, delta_t); 109 | freq = arFilter->getFrequencyHz(); 110 | } else if (tracker == Kalm_ANF) { 111 | double e; 112 | double f_kalmanANF = kalmANF->process((double)a_noisy, (double)delta_t, &e); 113 | freq = f_kalmanANF; 114 | } else { 115 | float f_byZeroCross = freqDetector->update(a_noisy, ZERO_CROSSINGS_SCALE /* max fractions of g */, 116 | ZERO_CROSSINGS_DEBOUNCE_TIME, ZERO_CROSSINGS_STEEPNESS_TIME, delta_t); 117 | if (f_byZeroCross == SCHMITT_TRIGGER_FREQ_INIT || f_byZeroCross == SCHMITT_TRIGGER_FALLBACK_FREQ) { 118 | freq = FREQ_GUESS; 119 | } else { 120 | freq = f_byZeroCross; 121 | } 122 | } 123 | return clamp(freq, FREQ_LOWER, FREQ_UPPER); 124 | } 125 | 126 | #endif 127 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-phi-A-symb.tex-part: -------------------------------------------------------------------------------- 1 | \subsection{Discrete Transition $\Phi_{AA}(h)=e^{F_{AA}h}$ for Attitude Error and Gyro Bias} 2 | 3 | \paragraph{Error-state model (right-multiplicative, small angle).} 4 | Let the attitude error be the small vector $\delta\boldsymbol{\theta}\in\mathbb{R}^3$ (so that the 5 | right-multiplicative correction quaternion is $\delta q(\delta\boldsymbol{\theta})$), and let 6 | $\delta\mathbf{b}_g\in\mathbb{R}^3$ be the gyroscope bias error. Over a short interval of length 7 | $h$ we assume the true body angular rate $\boldsymbol{\omega}$ is constant. The linearized 8 | continuous-time error dynamics are 9 | \[ 10 | \dot{\delta\boldsymbol{\theta}} 11 | = -[\boldsymbol{\omega}]_\times\,\delta\boldsymbol{\theta}\;-\;\delta\mathbf{b}_g, 12 | \qquad 13 | \dot{\delta\mathbf{b}}_g=\mathbf{0}, 14 | \] 15 | where $[\boldsymbol{\omega}]_\times$ is the $3\times3$ skew-symmetric matrix 16 | \[ 17 | [\boldsymbol{\omega}]_\times \;=\; 18 | \begin{bmatrix} 19 | 0 & -\omega_z & \omega_y\\ 20 | \omega_z & 0 & -\omega_x\\ 21 | -\omega_y & \omega_x & 0 22 | \end{bmatrix}. 23 | \] 24 | In block form, 25 | \[ 26 | \frac{d}{dt} 27 | \begin{bmatrix} 28 | \delta\boldsymbol{\theta}\\[2pt] \delta\mathbf{b}_g 29 | \end{bmatrix} 30 | = 31 | \underbrace{\begin{bmatrix} 32 | -[\boldsymbol{\omega}]_\times & -I\\ 33 | 0 & 0 34 | \end{bmatrix}}_{F_{AA}} 35 | \begin{bmatrix} 36 | \delta\boldsymbol{\theta}\\[2pt] \delta\mathbf{b}_g 37 | \end{bmatrix}. 38 | \] 39 | 40 | \paragraph{Block–triangular exponential.} 41 | Since $F_{AA}$ is block upper triangular, its exponential has the standard form 42 | \[ 43 | \begin{aligned} 44 | e^{F_{AA}s} \;&=\; 45 | \begin{bmatrix} 46 | e^{-[\boldsymbol{\omega}]_\times s} & 47 | \displaystyle \int_0^s e^{-[\boldsymbol{\omega}]_\times(s-u)}\,(-I)\,e^{0\cdot u}\,du\\[6pt] 48 | 0 & I 49 | \end{bmatrix}\\ 50 | &= 51 | \begin{bmatrix} 52 | R(s) & B(s)\\ 53 | 0 & I 54 | \end{bmatrix}. 55 | \end{aligned} 56 | \] 57 | Thus $\Phi_{AA}(h)=e^{F_{AA}h}$ is fully determined by 58 | \[ 59 | \begin{aligned} 60 | R(h)&=e^{-[\boldsymbol{\omega}]_\times h},\qquad\\ 61 | B(h)&=\int_0^h e^{-[\boldsymbol{\omega}]_\times(h-u)}(-I)\,du \;=\; -\int_0^h e^{-[\boldsymbol{\omega}]_\times u}\,du. 62 | \end{aligned} 63 | \] 64 | 65 | \paragraph{Closed form for $R(h)$ (Rodrigues formula).} 66 | Let $W\equiv [\boldsymbol{\omega}]_\times$, $\omega=\|\boldsymbol{\omega}\|$, and 67 | $\theta=\omega h$. Using the series 68 | $e^{-W h}=I-Wh+\tfrac{1}{2!}(Wh)^2-\tfrac{1}{3!}(Wh)^3+\cdots$ and the identities 69 | $W^3=-\omega^2 W$, $W^4=-\omega^2 W^2$, one can regroup into a linear combination of 70 | $I$, $W$, and $W^2$ with scalar coefficients that depend only on $\theta$: 71 | \[ 72 | \boxed{ \; 73 | R(h)=e^{-Wh} 74 | = I \;-\; \sin\theta\,\frac{W}{\omega} \;+\; (1-\cos\theta)\,\frac{W^2}{\omega^2}. 75 | \;} 76 | \] 77 | Equivalently, if $K=W/\omega=[\hat{\boldsymbol{\omega}}]_\times$ with 78 | $\hat{\boldsymbol{\omega}}=\boldsymbol{\omega}/\omega$, then 79 | \[ 80 | R(h)=I-\sin\theta\,K+(1-\cos\theta)K^2. 81 | \] 82 | 83 | \paragraph{Closed form for $B(h)$ (integral of the exponential).} 84 | We need $B(h)=-\int_0^h e^{-W u}\,du$. Using the Rodrigues form for $e^{-W u}$, 85 | \[ 86 | e^{-W u}=I-\sin(\omega u)\,\frac{W}{\omega} + \left(1-\cos(\omega u)\right)\frac{W^2}{\omega^2}, 87 | \] 88 | integrate termwise over $u\in[0,h]$: 89 | \[ 90 | \int_0^h e^{-W u}\,du 91 | = \;h\,I \;-\; \frac{1-\cos\theta}{\omega^2}\,W \;+\; \frac{h-\frac{\sin\theta}{\omega}}{\omega^2}\,W^2. 92 | \] 93 | Therefore 94 | \[ 95 | \boxed{\; 96 | \begin{aligned} 97 | B(h) \;&=\; -\int_0^h e^{-W u}\,du\\ 98 | \;&=\; -\Bigg[ 99 | h\,I \;-\; \frac{1-\cos\theta}{\omega^2}\,W \;+\; \frac{h-\frac{\sin\theta}{\omega}}{\omega^2}\,W^2 100 | \Bigg]. 101 | \; 102 | \end{aligned}} 103 | \] 104 | 105 | \paragraph{Assembled discrete transition for the attitude block.} 106 | Putting the pieces together, 107 | \[ 108 | \fitcol{ 109 | \boxed{ 110 | \begin{aligned} 111 | \Phi_{AA}(h) 112 | &= 113 | \exp(F_{AA}h) 114 | = 115 | \begin{bmatrix} 116 | R(h) & B(h)\\ 117 | 0 & I 118 | \end{bmatrix}, 119 | \qquad\\ 120 | R(h)&=I-\sin\theta\,\frac{W}{\omega}+(1-\cos\theta)\frac{W^2}{\omega^2},\\ 121 | B(h)&=-\Big[h\,I - \frac{1-\cos\theta}{\omega^2}W + \frac{h-\frac{\sin\theta}{\omega}}{\omega^2}W^2\Big] 122 | \end{aligned}, 123 | } 124 | } 125 | \] 126 | with $W=[\boldsymbol{\omega}]_\times$, $\omega=\|\boldsymbol{\omega}\|$, $\theta=\omega h$. 127 | 128 | \paragraph{Small–rate (series) limit for numerical robustness.} 129 | When $\omega\to 0$ (or $\theta\ll 1$), use the Maclaurin expansions 130 | $\sin\theta=\theta-\theta^3/6+\cdots$, $\cos\theta=1-\theta^2/2+\theta^4/24-\cdots$ to obtain 131 | \[ 132 | \begin{aligned} 133 | R(h)&=I - Wh + \tfrac{1}{2}W^2 h^2 \;+\; \mathcal{O}(h^3), 134 | \qquad\\ 135 | B(h)&= -\Big[h\,I - \tfrac{1}{2}W h^2 + \tfrac{1}{6}W^2 h^3\Big] \;+\; \mathcal{O}(h^4). 136 | \end{aligned} 137 | \] 138 | These series are algebraically equivalent to the closed forms above and avoid loss of 139 | significance for very small $\theta$. 140 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/AranovskiyFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef ARANOVSKIY_FILTER_H 2 | #define ARANOVSKIY_FILTER_H 3 | 4 | #include 5 | #include 6 | 7 | /* 8 | Copyright 2024-2025, Mikhail Grushinskiy 9 | 10 | Aranovskiy frequency estimator (C++ version) 11 | 12 | Reference: 13 | Alexey A. Bobtsov, Nikolay A. Nikolaev, Olga V. Slita, Alexander S. Borgul, Stanislav V. Aranovskiy 14 | "The New Algorithm of Sinusoidal Signal Frequency Estimation", 15 | 11th IFAC International Workshop on Adaptation and Learning in Control and Signal Processing, July 2013 16 | 17 | This is a nonlinear adaptive observer that tracks the frequency and phase of a sinusoidal signal. 18 | */ 19 | 20 | template 21 | class AranovskiyFilter { 22 | public: 23 | // Parameters 24 | Real a = Real(1); // Input filter gain 25 | Real b = Real(1); // Input filter gain 26 | Real k = Real(8); // Adaptive gain 27 | 28 | // State 29 | Real y = Real(0); // Last measurement 30 | Real x1 = Real(0); // Internal filtered state 31 | Real theta = Real(-0.09);// Estimator variable 32 | Real sigma = Real(-0.09);// Estimator variable 33 | Real x1_dot = Real(0); 34 | Real sigma_dot = Real(0); 35 | Real omega = Real(3.0); // Estimated angular frequency (rad/s) 36 | Real f = Real(0); // Estimated frequency (Hz) 37 | Real phase = Real(0); // Estimated phase (radians) 38 | 39 | // Internal time scaling for better low-frequency stability 40 | static constexpr Real TIME_SCALE = Real(20); // can be tuned (20x faster internal clock) 41 | 42 | // Constructor 43 | AranovskiyFilter(Real omega_up = Real(0.5) * 2 * M_PI, 44 | Real gain = Real(8), 45 | Real x1_0 = Real(0), 46 | Real theta_0 = Real(-0.09), 47 | Real sigma_0 = Real(-0.09)) 48 | { 49 | setParams(omega_up, gain); 50 | setState(x1_0, theta_0, sigma_0); 51 | } 52 | 53 | // Set filter parameters 54 | void setParams(Real omega_up, Real gain) { 55 | a = TIME_SCALE * omega_up; 56 | b = TIME_SCALE * omega_up; 57 | k = TIME_SCALE * gain; 58 | } 59 | 60 | // Set initial state 61 | void setState(Real x1_init, Real theta_init, Real sigma_init) { 62 | x1 = x1_init; 63 | theta = TIME_SCALE * TIME_SCALE * theta_init; 64 | sigma = TIME_SCALE * TIME_SCALE * sigma_init; 65 | y = Real(0); 66 | 67 | omega = std::sqrt(std::max(Real(1e-12), std::abs(theta))); 68 | f = omega / (Real(2) * M_PI); 69 | phase = Real(0); 70 | } 71 | 72 | // Update filter with new measurement and time step 73 | void update(Real y_meas, Real dt) { 74 | const Real delta_t = dt / TIME_SCALE; 75 | 76 | if (!std::isfinite(y_meas)) { 77 | return; 78 | } 79 | 80 | y = y_meas; 81 | 82 | // First-order low-pass filter 83 | x1_dot = -a * x1 + b * y; 84 | 85 | // Signal energy check (x1 ≈ filtered y) 86 | // signal_energy = x1^2 + y^2 + eps, with FMA for x1^2 + y^2 87 | const Real signal_energy = std::fma(x1, x1, y * y) + Real(1e-12); // avoid divide-by-zero 88 | const Real gain_scaling = signal_energy / (signal_energy + Real(1e-6)); // soften updates on low-energy signals 89 | 90 | // Nonlinear adaptation law (scaled by gain_scaling) 91 | // 92 | // Original: 93 | // phi = x1^2 * theta + a * x1 * x1_dot + b * x1_dot * y 94 | // Factorization: 95 | // phi = x1^2 * theta + x1_dot * (a * x1 + b * y) 96 | // 97 | const Real x1_sq = x1 * x1; 98 | const Real tmp = std::fma(b, y, a * x1); // a * x1 + b * y 99 | const Real phi = std::fma(x1_dot, tmp, x1_sq * theta);// x1_sq * theta + x1_dot * tmp 100 | 101 | const Real update_term = -k * phi * gain_scaling; 102 | 103 | sigma_dot = clamp_value(update_term, Real(-1e7), Real(1e7)); 104 | 105 | // Update theta and omega 106 | theta = std::fma(k * b * x1, y, sigma); // theta = sigma + k * b * x1 * y 107 | 108 | omega = std::sqrt(std::max(Real(1e-7), std::abs(theta))); 109 | f = omega / (Real(2) * M_PI); 110 | 111 | // State integration (internal scaled time) 112 | x1 += x1_dot * delta_t; 113 | sigma += sigma_dot * delta_t; 114 | 115 | // Phase estimation: phase of (x1 + j y) 116 | phase = std::atan2(x1, y); 117 | } 118 | 119 | // Get current frequency estimate (Hz) 120 | Real getFrequencyHz() const { 121 | return f / TIME_SCALE; 122 | } 123 | 124 | // Get current angular frequency estimate (rad/s, *internal* scaled) 125 | Real getOmega() const { 126 | return omega; 127 | } 128 | 129 | // Get current oscillator phase estimate (radians) 130 | Real getPhase() const { 131 | return phase; 132 | } 133 | 134 | private: 135 | static constexpr Real clamp_value(Real val, Real low, Real high) { 136 | return (val < low) ? low : (val > high ? high : val); 137 | } 138 | }; 139 | 140 | #endif // ARANOVSKIY_FILTER_H 141 | 142 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/freq_tracking_zero_crossing.tex: -------------------------------------------------------------------------------- 1 | \documentclass[11pt,letterpaper]{article} 2 | \usepackage{amsmath,amssymb,geometry,hyperref} 3 | \usepackage{fullpage} 4 | \usepackage{cite} 5 | \geometry{margin=1in} 6 | \begin{document} 7 | 8 | \title{Zero‐Crossing Frequency Tracking with Hysteresis and Debouncing} 9 | \author{Mikhail Grushinskiy} 10 | \date{July 2025} 11 | \maketitle 12 | 13 | \begin{abstract} 14 | We present a robust zero‐crossing frequency detector employing a Schmitt‐trigger hysteresis, debounce filtering, and cycle‐averaged period estimation. The method, implemented in \texttt{SchmittTriggerFrequencyDetector} (see \texttt{SchmittTriggerFrequencyDetector.h}), yields instantaneous frequency estimates, phase, and quality metrics (confidence, jitter, amplitude ratio, fallback status). 15 | \end{abstract} 16 | 17 | \section{Problem Statement} 18 | Given a real‐valued, approximately sinusoidal input signal \(s(t)\) with unknown amplitude \(A\) and frequency \(f\), we wish to estimate \(f\) in real time while rejecting noise and drift. The detector must also report a phase estimate and confidence metrics. 19 | 20 | \section{Normalized Signal and Hysteresis} 21 | Define the normalized signal 22 | \[ 23 | x(t) \;=\; \frac{s(t)}{\lvert A\rvert}, 24 | \quad A = \max_t |s(t)|. 25 | \] 26 | We choose a hysteresis threshold \(h>0\), and set upper and lower thresholds 27 | \[ 28 | T_{\mathrm{up}} = +h,\quad T_{\mathrm{low}} = -h. 29 | \] 30 | At each sample, \(x(t)\) is compared to \(\pm h\) to detect transitions. 31 | 32 | \section{State Machine with Debouncing} 33 | The detector maintains a state \(\sigma \in \{\mathrm{LOW}, \mathrm{HIGH}\}\). Let \(\Delta t\) be the sample interval, \(\Delta t_{\mathrm{deb}}\) the debounce interval, and \(\Delta t_{\mathrm{steep}}\) the minimum time to guard against spurious crossings. Define the local time‐within‐cycle \(u(t)\), incremented by \(\Delta t\) each step. 34 | 35 | \subsection{Transition Conditions} 36 | A low‐to‐high transition at time \(t_i\) occurs if 37 | \[ 38 | x(t_i) > T_{\mathrm{up}}, 39 | \quad u(t_i) - u(t_{i-1}) > \Delta t_{\mathrm{steep}}, 40 | \quad u(t_i) - u(t_{i-1}^{\mathrm{cross}}) > \Delta t_{\mathrm{deb}}, 41 | \] 42 | and similarly for high‐to‐low using \(x(t) < T_{\mathrm{low}}\). Each crossing time is recorded as 43 | \[ 44 | t^{\mathrm{cross}} = u(t) - \tfrac12\,\bigl[u(t)-u(\text{last threshold cross})\bigr]. 45 | \] 46 | 47 | \section{Period and Frequency Estimation} 48 | Let \(n\) crossings occur at times \(t_1,\dots,t_n\). After observing \(M=2\,N_{\rm cycle}+1\) crossings (i.e.\ \(N_{\rm cycle}\) full periods), compute the cycle time 49 | \[ 50 | T_{\rm cycle} 51 | = \frac{t_{n} - t_{1}}{\,N_{\rm cycle}\,}, 52 | \] 53 | and estimate the instantaneous period 54 | \[ 55 | T = \frac{2}{n-1}\,(t_{n} - t_{1}), 56 | \] 57 | so that the frequency estimate is 58 | \[ 59 | \hat f = \frac{1}{T}. 60 | \] 61 | If fewer than two crossings have been observed, or if no crossing occurs for a time 62 | \(\Delta t_{\mathrm{fb}}\), we set a fallback frequency 63 | \(\displaystyle f_{\mathrm{fb}} = 10^{-2}\,\mathrm{Hz}.\) 64 | 65 | \section{Statistical Quality Metrics} 66 | Maintain a circular history \(\{T_k\}\) of the last \(K\) period estimates. Let 67 | \[ 68 | \overline{T} = \frac1K\sum_{k=1}^K T_k, 69 | \quad 70 | \sigma_T^2 = \frac1K\sum_{k=1}^K (T_k-\overline{T})^2. 71 | \] 72 | Define 73 | \[ 74 | \text{jitter} = \sigma_T, 75 | \quad 76 | \text{confidence} = \max\bigl(0,\,1 - \tfrac{\sigma_T}{\overline{T}}\bigr). 77 | \] 78 | Additionally, the amplitude‐to‐threshold ratio 79 | \(\displaystyle R_A = \frac{|A|}{h}\) is reported. 80 | 81 | \section{Phase Estimation} 82 | Between crossings, we compute the phase 83 | \[ 84 | \phi(t) \;=\; 2\pi\,\frac{u(t) - u(t_{n-1}^{\mathrm{cross}})}{T} 85 | \quad (\bmod\,2\pi), 86 | \] 87 | offset by \(\pi\) on downward transitions to ensure continuity. 88 | 89 | \section{Implementation in \texttt{SchmittTriggerFrequencyDetector}} 90 | Key parameters and defaults: 91 | \begin{itemize} 92 | \item Hysteresis \(h=0.1\). 93 | \item Debounce time \(\Delta t_{\mathrm{deb}}= \text{user‐supplied}\). 94 | \item Steepness time \(\Delta t_{\mathrm{steep}}= \text{user‐supplied}\). 95 | \item Fallback time \(\Delta t_{\mathrm{fb}}=60\,\mathrm{s}\). 96 | \item History length \(K=10\). 97 | \end{itemize} 98 | The class is defined in \texttt{SchmittTriggerFrequencyDetector.h}, with methods: 99 | \begin{itemize} 100 | \item \texttt{float update(…)} — processes each sample, updates state, returns \(\hat f\). 101 | \item \texttt{float getPhaseEstimate()} — returns \(\phi(t)\). 102 | \item \texttt{QualityMetrics getQualityMetrics()} — returns \(\{\mathrm{confidence},\mathrm{jitter},R_A,\mathrm{is\_fallback}\}\). 103 | \end{itemize} 104 | 105 | \section{Conclusion} 106 | This Schmitt‐trigger frequency tracker achieves robust, low‐jitter frequency estimates in real time, with built‐in noise immunity via hysteresis and debouncing, and provides useful diagnostics for estimator quality. 107 | \end{document} 108 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/NmeaXDR.h: -------------------------------------------------------------------------------- 1 | #ifndef NmeaXDR_h 2 | #define NmeaXDR_h 3 | 4 | /* 5 | XDR - Transducer Measurement 6 | https://gpsd.gitlab.io/gpsd/NMEA.html#_xdr_transducer_measurement 7 | https://www.eye4software.com/hydromagic/documentation/articles-and-howtos/handling-nmea0183-xdr/ 8 | 9 | Format: $--XDR,a,x.x,a,c--c, ..... *hh 10 | Example: 11 | 12 | $HCXDR,A,171,D,PITCH,A,-37,D,ROLL,G,367,,MAGX,G,2420,,MAGY,G,-8984,,MAGZ*41 13 | $SDXDR,C,23.15,C,WTHI*70 14 | 15 | Transducer Types: 16 | A - Angular displacement 17 | C - Temperature 18 | D - Depth 19 | F - Frequency 20 | H - Humidity 21 | N - Force 22 | P - Pressure 23 | R - Flow 24 | B - Absolute humidity 25 | G - Generic 26 | I - Current 27 | L - Salinity 28 | S - Switch, valve 29 | T - Tachometer 30 | U - Voltage 31 | V - Volume 32 | could be more 33 | 34 | Unit of measurement 35 | "" - could be empty! 36 | A - Amperes 37 | B - Bars | Binary 38 | C - Celsius 39 | D - Degrees 40 | H - Hertz 41 | I - liters/second 42 | K - Kelvin | Density, kg/m3 kilogram per cubic metre 43 | M - Meters | Cubic Meters (m3) 44 | N - Newton 45 | P - Percent of full range | Pascal 46 | R - RPM 47 | S - Parts per thousand 48 | V - Volts 49 | could be more 50 | */ 51 | 52 | /* 53 | Bareboat Necessities Sensors NMEA-0183 XDR Sentences: 54 | 55 | NMEA-0183 Sender 56 | BB 57 | 58 | Transducer name suffix: 59 | 1 - method #1 60 | 2 - method #2 61 | 62 | Heave (vertical displacement) 63 | Transducer type: D (Depth) 64 | Unit of measurement: M (meters) 65 | Transducer name prefix: 66 | DHI - displacement max 67 | DLO - displacement min 68 | DAV - displacement average (bias) 69 | DRT - displacement in real time 70 | DRG - displacement range 71 | 72 | Vertical acceleration (from observer point of view): 73 | Transducer type: N (Force) 74 | Unit of measurement: P - Percent of g (accel of free fall) 75 | Transducer name: 76 | AHI - vertical acceleration max 77 | ALO - vertical acceleration min 78 | ART - vertical acceleration in real time 79 | ARG - vertical acceleration range 80 | ABI - vertical acceleration sensor bias 81 | 82 | Heave frequency (from observer point of view): 83 | Transducer type: F (Frequency) 84 | Unit of measurement: H (Hertz) 85 | Transducer name: 86 | FHI - frequency max 87 | FLO - frequency min 88 | FRT - frequency in real time 89 | FAV - frequency average 90 | 91 | IMU Sample rate: 92 | Transducer type: F (Frequency) 93 | Unit of measurement: H (Hertz) 94 | Transducer name: 95 | SRT - sample rate 96 | 97 | Wave length (from observer point of view): 98 | Transducer type: G 99 | Unit of measurement: M (Meters) 100 | Transducer name: 101 | AP_WAVE_LENGTH - wave length 102 | 103 | Wave period (from observer point of view): 104 | Transducer type: G 105 | Unit of measurement: empty (seconds) 106 | Transducer name: 107 | AP_WAVE_PERIOD - wave period 108 | 109 | Wave angle (from observer point of view): 110 | Transducer type: A 111 | Unit of measurement: D (degrees) 112 | Transducer name: 113 | AP_WAVE_ANGLE - wave angle 114 | 115 | Wave direction (from observer point of view): 116 | Transducer type: G 117 | Unit of measurement: none 118 | Transducer name: 119 | AP_WAVE_DIR - wave direction (1 forward, -1 backward, 0 uncertain) 120 | 121 | Wave speed (from observer point of view): 122 | Transducer type: G 123 | Unit of measurement: empty (m/s) 124 | Transducer name: 125 | AP_WAVE_SPEED - wave speed 126 | 127 | Examples: 128 | 129 | Accel: 130 | $BBXDR,N,100.0300,P,ARG1*NN 131 | $BBXDR,N,-0.001,P,ABI1*NN 132 | 133 | Freq: 134 | $BBXDR,F,0.2450,H,FAV1*NN 135 | $BBXDR,F,0.2500,H,FRT1*NN 136 | 137 | Heave: 138 | $BBXDR,D,0.1000,M,DHI2*NN 139 | $BBXDR,D,-0.1000,M,DLO2*NN 140 | $BBXDR,D,0.2000,M,DRG1*NN 141 | $BBXDR,D,0.0,M,DAV1*NN 142 | $BBXDR,D,-0.0030,M,DRT2*NN 143 | 144 | */ 145 | 146 | #include "NmeaChecksum.h" 147 | 148 | void gen_nmea0183_xdr(const char *nmea_fmt, float value) { 149 | char nmea_part[82]; 150 | snprintf(nmea_part, 76, nmea_fmt, value); 151 | int checksum = nmea0183_checksum(nmea_part); 152 | Serial.printf("%s*%02X\r\n", nmea_part, checksum); 153 | } 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/wave_profile_and_predictor.tex: -------------------------------------------------------------------------------- 1 | \documentclass[11pt,letterpaper]{article} 2 | \usepackage{amsmath,amssymb} 3 | \usepackage{authblk} 4 | \usepackage{graphicx} 5 | \usepackage{fullpage} 6 | \usepackage{cite} 7 | 8 | \title{Real‐Time Wave Surface Profiling and Prediction} 9 | \author{Mikhail Grushinskiy} 10 | \affil{Independent Researcher, 2025} 11 | 12 | \begin{document} 13 | \maketitle 14 | 15 | \begin{abstract} 16 | This paper describes \texttt{WaveSurfaceProfile}, a rolling‐buffer heave tracker that reconstructs the instantaneous wave shape (phase, crest sharpness, asymmetry), computes derived metrics (energy, Stokes drift, velocity gradient), and predicts future heave at arbitrary phase offsets. Use cases include time‐domain wave monitoring, real‐time control of wave‐active systems, and offline spectral (FFT) analysis of stored profiles. 17 | \end{abstract} 18 | 19 | \section{Introduction} 20 | Accurate knowledge of the instantaneous wave profile is critical for coastal monitoring, maritime navigation, and wave energy conversion. Here is presented a lightweight C++ template class that: 21 | \begin{itemize} 22 | \item Maintains a circular buffer of the last \(N\) heave samples. 23 | \item Anchors phase to the most recent zero upcrossing. 24 | \item Computes shape metrics: crest sharpness, asymmetry. 25 | \item Predicts heave at future or past phase offsets. 26 | \item Exports buffer for FFT‐based spectral analysis. 27 | \end{itemize} 28 | 29 | \section{Data Buffer and Phase Tracking} 30 | Store samples \(\{(t_i,\eta_i)\}_{i=1}^N\) in a ring buffer. On each \texttt{update(heave,freq,t)}: 31 | \[ 32 | \text{head}\leftarrow(\text{head}+1)\bmod N,\quad 33 | \text{samples}[\text{head}]=(\eta,t), 34 | \quad \nu\leftarrow\begin{cases} 35 | \nu, & \nu\in(0.01,2.0)\,\text{Hz},\\ 36 | \nu, & \text{else unchanged}. 37 | \end{cases} 38 | \] 39 | Phase \(\phi(t)\in[0,1)\) is defined by 40 | \[ 41 | \phi=\bigl(t - t_{\rm ZCU}\bigr)\,\nu \bmod 1, 42 | \] 43 | where \(t_{\rm ZCU}\) is the interpolated time of the most recent zero upcrossing (\(\eta<0\to\eta\ge0\)). 44 | 45 | \section{Shape Metrics} 46 | 47 | \subsection{Crest Sharpness} 48 | Locate the last upcrossing \(t_u\), crest peak \(\max\eta\) at \(t_c\), and subsequent downcrossing \(t_d\). Define 49 | \[ 50 | \text{sharpness} 51 | = \tfrac12\Bigl(\tfrac{\eta_{\max}}{t_c - t_u} + \tfrac{\eta_{\max}}{t_d - t_c}\Bigr). 52 | \] 53 | 54 | \subsection{Asymmetry} 55 | \[ 56 | \text{asymmetry} 57 | = \frac{(t_c - t_u)-(t_d-t_c)}{(t_c - t_u)+(t_d - t_c)}. 58 | \] 59 | 60 | \section{Prediction at Arbitrary Phase} 61 | To predict heave at phase offset \(\Delta\phi\): 62 | \begin{enumerate} 63 | \item Compute current phase \(\phi_0\). 64 | \item Target phase \(\phi^*=\phi_0+\Delta\phi\) (mod 1). 65 | \item Scan buffer segments \((\eta_i,\eta_{i+1})\), compute their phases \(\phi_i,\phi_{i+1}\). 66 | \item If \(\phi^*\) lies between \(\phi_i,\phi_{i+1}\), linearly interpolate: 67 | \(\eta^*=\eta_i + \frac{\phi^*-\phi_i}{\phi_{i+1}-\phi_i}(\eta_{i+1}-\eta_i)\). 68 | \item Otherwise, choose the nearest endpoint. 69 | \end{enumerate} 70 | 71 | \section{Derived Physical Quantities} 72 | 73 | \paragraph{Wave Energy (J/m²):} 74 | \[ 75 | E = \tfrac12\,\rho g\,\frac1N\sum_{i=1}^N \eta_i^2. 76 | \] 77 | 78 | \paragraph{Stokes Drift (m/s):} 79 | For deep water at surface, 80 | \[ 81 | U_s = \omega k a^2,\quad 82 | a = \sqrt{\frac1N\sum\eta_i^2}\,\sqrt{2},\quad 83 | k=\frac{\omega^2}{g}. 84 | \] 85 | 86 | \paragraph{Velocity Gradient (m/s²):} 87 | \[ 88 | G = \frac{\max\eta-\min\eta}{\Delta t_{\max}-\Delta t_{\min}}\;\times\;\frac{g}{2\pi\nu}. 89 | \] 90 | 91 | \section{FFT and Spectral Analysis} 92 | By exporting the buffered \(\{\eta_i\}\) array, one can perform an \(N\)-point FFT to extract: 93 | \[ 94 | S(f) = \bigl|\mathrm{FFT}(\eta_i)\bigr|^2, 95 | \] 96 | for wave‐height spectra, peak period detection, and directional spreading (with multi‐axis data). 97 | 98 | \section{Use Cases} 99 | \begin{itemize} 100 | \item \textbf{Real‐Time Monitoring:} Compute phase–locked metrics for control of wave‐energy converters. 101 | \item \textbf{Forecasting:} Short‐term heave prediction for vessel motion compensation. 102 | \item \textbf{Spectral Analysis:} Offline FFT of stored profiles for research and regulatory reporting. 103 | \item \textbf{Anomaly Detection:} Detect unusual crest behavior or asymmetry spikes indicating rogue waves. 104 | \end{itemize} 105 | 106 | \section{Implementation Notes} 107 | \begin{itemize} 108 | \item Handling finite‐precision: samples with non‐monotonic timestamps are discarded. 109 | \item Interpolation safety: phase wrapping and clamping guard against division by zero. 110 | \item Storage strategy: configurable \(N\) (default 128) and \(\text{STORE\_PERIODS}=2\) periods. 111 | \end{itemize} 112 | 113 | \section{Conclusion} 114 | \texttt{WaveSurfaceProfile} provides a compact, efficient framework for rolling wave shape analysis, real‐time prediction, and spectral export, enabling advanced wave sensing on embedded platforms. 115 | 116 | \end{document} 117 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/ahrs_kalman_qmekf.tex: -------------------------------------------------------------------------------- 1 | \documentclass[11pt,letterpaper]{article} 2 | \usepackage{amsmath,amssymb,amsthm,fullpage} 3 | \usepackage{hyperref} 4 | \usepackage{cite} 5 | 6 | \title{A Quaternion‐Multiplicative Extended Kalman Filter for Attitude Estimation:\\ 7 | Mathematical Formulation and Analysis} 8 | \author{} 9 | \date{} 10 | 11 | \begin{document} 12 | \maketitle 13 | 14 | \begin{abstract} 15 | This paper presents a formulation of the Quaternion‐MEKF exactly matching the provided C++ implementation. Key aspects include: use of the exact quaternion exponential map for propagation (Shuster integration~\cite{Shuster1981}), a 6‐state error vector with gyro bias, full 6×6 covariance, Joseph‐form update for numerical robustness, and measurement Jacobians consistent with accelerometer and magnetometer cross‐product conventions. Noise covariances are chosen directly from sensor noise densities and bias instability. 16 | \end{abstract} 17 | 18 | \section{State Representation} 19 | Define the estimate quaternion $\hat q\in\mathbb H$ and error state 20 | \[ 21 | \delta x = \begin{bmatrix}\delta\theta\\ b\end{bmatrix}\in\mathbb R^6, 22 | \quad 23 | \delta q \approx \begin{bmatrix}1\\ \tfrac12\,\delta\theta\end{bmatrix}, 24 | \] 25 | where $\delta\theta$ is the small‐angle error and $b$ the gyro‐bias error~\cite{Lefferts1982,Markley2003}. 26 | 27 | \section{Propagation via Exact Exponential} 28 | Measured rate is $\omega_m=\omega+\nu_g - b$. The code forms the 4×4 quaternion‐update matrix via the exact exponential map 29 | \[ 30 | \Delta q 31 | =\exp\!\bigl(\tfrac12\,\Omega(\Delta\theta)\bigr) 32 | =\cos\!\bigl(\tfrac12\|\Delta\theta\|\bigr)\,I_4 33 | \;+\;\frac{\sin(\tfrac12\|\Delta\theta\|)}{\|\Delta\theta\|}\,\Omega(\Delta\theta), 34 | \] 35 | where $\Delta\theta=\omega_m\,\Delta t$ and 36 | \[ 37 | \Omega(\Delta\theta) 38 | =\begin{bmatrix} 39 | 0 & -\Delta\theta^\mathsf{T}\\[3pt] 40 | \Delta\theta & -[\Delta\theta]_\times 41 | \end{bmatrix}. 42 | \] 43 | Propagation and renormalization are then 44 | \[ 45 | \hat q^-=\Delta q\,\hat q, 46 | \qquad 47 | \hat q\leftarrow\frac{\hat q^-}{\|\hat q^-\|}. 48 | \] 49 | 50 | The 6×6 discrete‐time transition for error plus bias is 51 | \[ 52 | F = 53 | \begin{bmatrix} 54 | F_{11} & -I_3\,\Delta t\\[3pt] 55 | 0_{3\times3} & I_3 56 | \end{bmatrix}, 57 | \quad 58 | F_{11} = \bigl(\Delta q\bigr)_{0\!:\!2,\;0\!:\!2}. 59 | \] 60 | 61 | \section{Process Noise Covariance} 62 | The filter uses 63 | \[ 64 | Q = \operatorname{diag}\bigl(\sigma_g^2\,\Delta t^2,\;\sigma_b^2\,\Delta t\bigr), 65 | \] 66 | matching the code’s \texttt{initialize\_Q}. 67 | 68 | \section{Measurement Model and Jacobian} 69 | Accelerometer and magnetometer give 70 | \[ 71 | y = \begin{bmatrix}a_m\\ m_m\end{bmatrix} 72 | = 73 | \begin{bmatrix} 74 | \hat q^{-1}\otimes 75 | \begin{pmatrix}0 \\ \mathbf v_g\end{pmatrix} 76 | \otimes\hat q 77 | \\[8pt] 78 | \hat q^{-1}\otimes 79 | \begin{pmatrix}0 \\ \mathbf v_m\end{pmatrix} 80 | \otimes\hat q 81 | \end{bmatrix} 82 | + \nu, 83 | \] 84 | where $\mathbf v_g=(0,0,-g)^\mathsf{T}$ and $\mathbf v_m$ is the reference magnetic field. The linearization uses 85 | \[ 86 | C = 87 | \begin{bmatrix} 88 | [\hat v_g]_\times & 0\\[3pt] 89 | [\hat v_m]_\times & 0 90 | \end{bmatrix}, 91 | \] 92 | so the innovation is $\nu_k=y_k-\hat y_k=C\,\delta x+v$ exactly as in the code. 93 | 94 | \section{Measurement Update with Joseph Form} 95 | Innovation covariance: 96 | \[ 97 | S = C\,P^-\,C^\top + R, 98 | \quad R=\operatorname{diag}(\sigma_a^2,\sigma_m^2). 99 | \] 100 | Kalman gain and Joseph‐form update: 101 | \[ 102 | K = P^-C^\top S^{-1},\quad 103 | P = (I - K\,C)\,P^-\,(I - K\,C)^\top \;+\; K\,R\,K^\top. 104 | \] 105 | After correction $x\leftarrow x + K\nu$, the quaternion is updated by 106 | \[ 107 | \hat q \leftarrow \hat q \;\otimes\; 108 | \begin{pmatrix}1\\ \tfrac12\,\delta\theta\end{pmatrix}, 109 | \quad 110 | \delta\theta\leftarrow0. 111 | \] 112 | 113 | \section{Numerical Considerations} 114 | \begin{itemize} 115 | \item \textbf{Exact Exponential:} preserves second‐order accuracy (Shuster integration~\cite{Shuster1981}). 116 | \item \textbf{Joseph Form:} maintains symmetry and positive definiteness in floating‐point~\cite{Maybeck1979}. 117 | \item \textbf{Renormalization:} enforces $\|\hat q\|=1$ each cycle. 118 | \end{itemize} 119 | 120 | \begin{thebibliography}{9} 121 | \bibitem{Lefferts1982} 122 | E.~J. Lefferts, F.~L. Markley, and M.~D. Shuster, “Kalman filtering for spacecraft attitude estimation,” \emph{J. Guidance, Control, and Dynamics}, vol.~5, no.~5, pp. 417–429, 1982. 123 | 124 | \bibitem{Markley2003} 125 | F.~L. Markley, “Attitude error representations for Kalman filtering,” \emph{J. Guidance, Control, and Dynamics}, vol.~26, no.~2, pp. 311–317, 2003. 126 | 127 | \bibitem{Shuster1981} 128 | M.~D. Shuster and S.~D. Oh, “Three‐axis attitude determination from vector observations,” \emph{J. Guidance, Control, and Dynamics}, vol.~4, no.~1, pp. 70–77, 1981. 129 | 130 | \bibitem{Maybeck1979} 131 | P.~S. Maybeck, \emph{Stochastic Models, Estimation, and Control}, vol.~1. Academic Press, 1979. 132 | 133 | \end{thebibliography} 134 | \end{document} 135 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-propag.tex-part: -------------------------------------------------------------------------------- 1 | \section{Process Matrix and Blockwise Propagation} 2 | 3 | The extended filter operates on a composite state 4 | \[ 5 | x = 6 | \begin{bmatrix} 7 | \delta\boldsymbol{\theta} & 8 | \mathbf{b}_g & 9 | \mathbf{v} & 10 | \mathbf{p} & 11 | \mathbf{S} & 12 | \mathbf{a}_w & 13 | \mathbf{b}_a 14 | \end{bmatrix}^{\!\top}, 15 | \] 16 | whose dynamics couple rotational, translational, and bias subsystems. 17 | The corresponding discrete transition matrix 18 | $\Phi \in \mathbb{R}^{N_X\times N_X}$ is organized in block form: 19 | \[ 20 | \Phi = 21 | \begin{bmatrix} 22 | \Phi_{\!AA} & \Phi_{\!AB} & \Phi_{\!AL} \\ 23 | 0 & \Phi_{\!BB} & 0 \\ 24 | 0 & 0 & \Phi_{\!LL} 25 | \end{bmatrix}, 26 | \] 27 | where the subblocks represent: 28 | \begin{itemize} 29 | \item $\Phi_{\!AA}$ — attitude error and gyro-bias propagation; 30 | \item $\Phi_{\!BB}$ — accelerometer-bias random walk (identity); 31 | \item $\Phi_{\!LL}$ — linear translational subsystem 32 | $[\mathbf{v},\mathbf{p},\mathbf{S},\mathbf{a}_w]$; 33 | \item $\Phi_{\!AB}$ and $\Phi_{\!AL}$ — cross-covariance couplings between 34 | attitude, biases, and linear states. 35 | \end{itemize} 36 | 37 | \subsection{Attitude and Gyro-Bias Blocks} 38 | 39 | For a small rotation increment $\boldsymbol{\omega}h$, 40 | the local attitude linearization is 41 | \begin{equation} 42 | \Phi_{\!AA} 43 | = 44 | \begin{bmatrix} 45 | \mathbf{I}_3 - [\boldsymbol{\omega}]_\times h 46 | + \tfrac{1}{2}[\boldsymbol{\omega}]_\times^2 h^2 & 47 | -\mathbf{I}_3 h \\[3pt] 48 | \mathbf{0}_{3\times3} & \mathbf{I}_3 49 | \end{bmatrix}. 50 | \end{equation} 51 | The upper-right term $-\mathbf{I}_3 h$ couples gyro-bias uncertainty into 52 | attitude error over the integration step. 53 | 54 | \subsection{Linear Translational Block} 55 | 56 | Each axis of the OU-driven translational chain follows 57 | \[ 58 | \dot{\mathbf{x}}_L 59 | = \mathbf{A}_L \mathbf{x}_L + \mathbf{w}, 60 | \qquad 61 | \mathbf{x}_L = [v,\,p,\,S,\,a_w]^\top, 62 | \] 63 | whose exact discrete transition over~$h$ is 64 | \begin{equation} 65 | \Phi_{\!LL} 66 | = 67 | \begin{bmatrix} 68 | \mathbf{I}_3 & 0 & 0 & \phi_{va}\mathbf{I}_3\\[2pt] 69 | h\mathbf{I}_3 & \mathbf{I}_3 & 0 & \phi_{pa}\mathbf{I}_3\\[2pt] 70 | \tfrac{1}{2}h^2\mathbf{I}_3 & h\mathbf{I}_3 & \mathbf{I}_3 & \phi_{Sa}\mathbf{I}_3\\[2pt] 71 | 0 & 0 & 0 & e^{-h/\tau_{a_w}}\mathbf{I}_3 72 | \end{bmatrix}. 73 | \end{equation} 74 | The coefficients $(\phi_{va},\phi_{pa},\phi_{Sa})$ are given by the 75 | closed-form expressions in~(\ref{eq:ou-chain}) and switch to Maclaurin 76 | series when $h/\tau_{a_w}$ is small. 77 | 78 | \subsection{Full Covariance Propagation} 79 | 80 | Denoting by $P$ the covariance matrix of the entire state, 81 | the discrete prediction step partitions $P$ into the same block structure: 82 | \[ 83 | P = 84 | \begin{bmatrix} 85 | P_{AA} & P_{AL} \\ 86 | P_{AL}^\top & P_{LL} 87 | \end{bmatrix}, 88 | \] 89 | where $P_{AA}$ covers attitude and biases, and $P_{LL}$ covers the 90 | linear subsystem. 91 | 92 | The covariance update is then applied block-by-block: 93 | \begin{align} 94 | P_{AA}^{+} 95 | &= \Phi_{\!AA} P_{AA} \Phi_{\!AA}^\top + Q_{AA},\\ 96 | P_{LL}^{+} 97 | &= \Phi_{\!LL} P_{LL} \Phi_{\!LL}^\top + Q_{LL},\\ 98 | P_{AL}^{+} 99 | &= \Phi_{\!AA} P_{AL} \Phi_{\!LL}^\top. 100 | \end{align} 101 | The cross term $P_{AL}$ preserves the correlation between attitude 102 | and translational uncertainties, ensuring that rotational misalignment 103 | feeds correctly into linear-motion uncertainty. 104 | 105 | When accelerometer biases are active, additional bias blocks are updated as 106 | \begin{equation} 107 | P_{BB}^{+} = P_{BB} + Q_{b_a} h, \qquad 108 | P_{AB}^{+} = \Phi_{\!AA} P_{AB}, \qquad 109 | P_{LB}^{+} = \Phi_{\!LL} P_{LB}, 110 | \end{equation} 111 | and their transposes restore overall symmetry. 112 | 113 | \subsection{Assembly of the Complete Transition} 114 | 115 | The complete transition matrix is therefore sparse and structured as 116 | \[ 117 | \Phi = 118 | \begin{bmatrix} 119 | \Phi_{\!AA} & 0 & 0\\ 120 | 0 & \mathbf{I}_{3} & 0\\ 121 | 0 & 0 & \Phi_{\!LL} 122 | \end{bmatrix} 123 | + 124 | \text{cross-coupling terms}, 125 | \] 126 | with a corresponding block-diagonal process noise 127 | \[ 128 | Q = 129 | \mathrm{diag}(Q_{AA},\,Q_{LL},\,Q_{b_a}), 130 | \] 131 | where $Q_{LL}$ is obtained from the OU discretization and $Q_{AA}$, 132 | $Q_{b_a}$ are derived from gyro and accelerometer random-walk spectra. 133 | 134 | \subsection{Symmetry and Positive-Semidefinite Projection} 135 | 136 | After each propagation, the covariance is symmetrized and 137 | projected to the nearest positive-semidefinite matrix to mitigate 138 | floating-point asymmetry: 139 | \[ 140 | P \leftarrow \tfrac{1}{2}(P + P^\top), \qquad 141 | P \succeq 0. 142 | \] 143 | Eigenvalue flooring ensures stability even under strong anisotropy or 144 | nearly singular correlation structures in~$Q_{LL}$. 145 | 146 | This blockwise formulation allows efficient numerical propagation, 147 | preserving the independence of rotational, translational, and bias 148 | subsystems while maintaining full cross-covariance consistency. 149 | 150 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-tupdate.tex-part: -------------------------------------------------------------------------------- 1 | \section{Time Update} 2 | 3 | At each sampling interval~$h$, the filter performs a discrete-time prediction 4 | that advances both attitude and translational states according to the system's 5 | continuous dynamics. The update couples the quaternion kinematics of body 6 | rotation with an Ornstein–Uhlenbeck (OU) process driving the latent 7 | world-frame acceleration. The procedure ensures consistent propagation of 8 | mean and covariance under finite sampling, while maintaining numerical 9 | stability for small~$h/\tau$. 10 | 11 | \subsection{Attitude Propagation} 12 | 13 | The body angular velocity measurement~$\boldsymbol{\omega}_b$ is corrected for 14 | the current bias estimate~$\mathbf{b}_g$, yielding the bias-compensated rate 15 | \[ 16 | \boldsymbol{\omega} 17 | = \boldsymbol{\omega}_b - \mathbf{b}_g. 18 | \] 19 | The quaternion describing the world-to-body rotation is advanced using the 20 | right-multiplicative exponential map 21 | \begin{equation} 22 | q_{k+1} = q_k \otimes 23 | \exp_q\!\left(\tfrac{1}{2}\,\boldsymbol{\omega}\,h\right), 24 | \end{equation} 25 | where $\exp_q(\cdot)$ denotes the quaternion exponential. 26 | For small rotation increments $\|\boldsymbol{\omega}h\|\!\ll\!1$, 27 | a second-order Maclaurin approximation is applied to avoid division by 28 | near-zero angles and loss of orthogonality. 29 | 30 | The corresponding local linearization of the attitude error is represented 31 | by the transition submatrix 32 | \[ 33 | \mathbf{F}_{\theta\theta} 34 | \simeq \mathbf{I}_3 - [\boldsymbol{\omega}]_\times h 35 | + \tfrac{1}{2}[\boldsymbol{\omega}]_\times^2 h^2, 36 | \] 37 | which retains the correct skew-symmetric structure and preserves covariance 38 | consistency in the small-angle limit. When the gyroscope bias is estimated, 39 | the cross-term $\mathbf{F}_{\theta b_g} = -\mathbf{I}_3 h$ 40 | accounts for coupling between bias uncertainty and orientation drift. 41 | 42 | \subsection{Linear Translational Propagation} 43 | 44 | The translational subsystem evolves under the OU-driven process 45 | \begin{equation} 46 | \dot{\mathbf{v}} = \mathbf{a}_w, \qquad 47 | \dot{\mathbf{p}} = \mathbf{v}, \qquad 48 | \dot{\mathbf{S}} = \mathbf{p}, \qquad 49 | \dot{\mathbf{a}}_w = -\tfrac{1}{\tau_{a_w}}\mathbf{a}_w 50 | + \boldsymbol{\eta}(t), 51 | \label{eq:ou-chain} 52 | \end{equation} 53 | where $\boldsymbol{\eta}(t)$ is zero-mean white noise with stationary 54 | covariance $\Sigma_{a_w}$. 55 | Exact moment matching over the interval~$h$ gives the discrete transition 56 | \[ 57 | \begin{bmatrix} 58 | \mathbf{v}\\[2pt]\mathbf{p}\\[2pt]\mathbf{S}\\[2pt]\mathbf{a}_w 59 | \end{bmatrix}_{k+1} 60 | = 61 | \Phi_{L}(h,\tau_{a_w}) 62 | \begin{bmatrix} 63 | \mathbf{v}\\[2pt]\mathbf{p}\\[2pt]\mathbf{S}\\[2pt]\mathbf{a}_w 64 | \end{bmatrix}_{k}, 65 | \] 66 | with block structure 67 | \begin{equation} 68 | \Phi_{L}(h,\tau) 69 | = 70 | \begin{bmatrix} 71 | \mathbf{I}_3 & 0 & 0 & \phi_{va}\mathbf{I}_3\\[3pt] 72 | h\mathbf{I}_3 & \mathbf{I}_3 & 0 & \phi_{pa}\mathbf{I}_3\\[3pt] 73 | \tfrac{1}{2}h^2\mathbf{I}_3 & h\mathbf{I}_3 & \mathbf{I}_3 & \phi_{Sa}\mathbf{I}_3\\[3pt] 74 | 0 & 0 & 0 & e^{-h/\tau}\mathbf{I}_3 75 | \end{bmatrix}. 76 | \end{equation} 77 | The coefficients $\phi_{va}$, $\phi_{pa}$, and $\phi_{Sa}$ are defined by 78 | \begin{align} 79 | \phi_{va} &= \tau(1 - e^{-h/\tau}),\\ 80 | \phi_{pa} &= \tau^2\!\left(\tfrac{h}{\tau} + e^{-h/\tau} - 1\right),\\ 81 | \phi_{Sa} &= \tau^3\!\left(\tfrac{1}{2}\!\left(\tfrac{h}{\tau}\right)^2 82 | - \tfrac{h}{\tau} - (e^{-h/\tau}-1)\right), 83 | \end{align} 84 | with high-order series expansions substituted when $h/\tau$ is small. 85 | 86 | \subsection{Discrete Process Covariance} 87 | 88 | The OU spectral density yields a closed-form discrete covariance for 89 | $\bigl[\mathbf{v},\mathbf{p},\mathbf{S},\mathbf{a}_w\bigr]$: 90 | \[ 91 | \mathbf{Q}_L(h,\tau,\Sigma_{a_w}) 92 | = \frac{2}{\tau}\, 93 | \Sigma_{a_w}\,\mathcal{K}(h,\tau), 94 | \] 95 | where $\mathcal{K}$ is the matrix of integrated exponential kernels. 96 | If $\Sigma_{a_w}$ contains horizontal–vertical correlations, 97 | the full $12\times12$ block covariance is constructed as 98 | \[ 99 | \mathbf{Q}_{LL} 100 | = \Sigma_{a_w}^{1/2} \otimes \mathbf{Q}_L^{(1)} 101 | \bigl(\Sigma_{a_w}^{1/2}\bigr)^\top, 102 | \] 103 | and projected to the nearest positive semidefinite form to maintain 104 | numerical stability. 105 | 106 | \subsection{Bias and Cross-Covariance Propagation} 107 | 108 | Bias states evolve as independent random walks with continuous power 109 | spectral densities $\mathbf{Q}_{b_g}$ and $\mathbf{Q}_{b_a}$. 110 | Their covariances are incremented linearly with~$h$, and cross terms with 111 | attitude and linear blocks are propagated using the same transition 112 | submatrices as in the main system. 113 | 114 | \subsection{Covariance Propagation and Symmetry Enforcement} 115 | 116 | Combining all subsystems, the prior covariance at step~$k{+}1$ is computed as 117 | \begin{equation} 118 | P_{k+1|k} 119 | = \Phi P_{k|k} \Phi^\top + Q, 120 | \end{equation} 121 | where $\Phi$ is block-diagonal in attitude and linear subsystems, 122 | and $Q$ contains $\mathbf{Q}_{LL}$ along with the bias random-walk terms. 123 | To counteract rounding asymmetry, the matrix is explicitly symmetrized: 124 | \[ 125 | P_{k+1|k} \leftarrow \tfrac{1}{2} 126 | (P_{k+1|k} + P_{k+1|k}^\top). 127 | \] 128 | This guarantees positive semidefiniteness of $P$ within numerical precision 129 | and prevents divergence of the filter during long autonomous propagation. 130 | 131 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/ins_diagram.tex: -------------------------------------------------------------------------------- 1 | \documentclass[letterpaper]{article} 2 | \usepackage[margin=1in]{geometry} % US Letter with 1-inch margins 3 | \usepackage{tikz} 4 | \usepackage{aeguill} 5 | 6 | \begin{document} 7 | 8 | \begin{center} 9 | \resizebox{\textwidth}{!}{% scale diagram to fit page width 10 | \begin{tikzpicture}[yscale=-1 11 | ,pstyle0/.style={color=black,fill=gray!10,line width=1.5pt} 12 | ,pstyle1/.style={color=black,line width=1.5pt} 13 | ,pstyle2/.style={color=black,fill=black,line width=1.5pt} 14 | ] 15 | 16 | % --- Rectangles with sharp corners --- 17 | \draw[pstyle0] (260pt,12pt) rectangle (506.2609pt,44.7461pt); 18 | \node at (270pt,17pt)[below right,color=black]{IMU}; 19 | 20 | \draw[pstyle0] (32.5pt,110pt) rectangle (733.5667pt,142.7461pt); 21 | \node at (42.5pt,115pt)[below right,color=black]{AHRS (Mahony, KalmanQMEKF)}; 22 | 23 | \draw[pstyle0] (289.5pt,208pt) rectangle (476.3973pt,311.7305pt); 24 | \node at (299.5pt,213pt)[below right,color=black]{Frequency Tracker:}; 25 | \node at (299.5pt,248.4922pt)[below right,color=black]{Aranovskiy}; 26 | \node at (299.5pt,266.2383pt)[below right,color=black]{Kalm\_ANF}; 27 | \node at (299.5pt,283.9844pt)[below right,color=black]{Zero Crossing}; 28 | 29 | \draw[pstyle0] (128pt,377pt) rectangle (330.3595pt,409.7461pt); 30 | \node at (138pt,382pt)[below right,color=black]{Kalman Wave Direction}; 31 | 32 | \draw[pstyle0] (436pt,377pt) rectangle (661.9957pt,409.7461pt); 33 | \node at (446pt,382pt)[below right,color=black]{Sea State Detection}; 34 | 35 | \draw[pstyle0] (304.5pt,573pt) rectangle (717.4414pt,658.9844pt); 36 | \node at (314.5pt,578pt)[below right,color=black]{Kalman Heave Reconstruction}; 37 | \node at (314.5pt,613.4922pt)[below right,color=black]{Kalman Integration with Drift Correction}; 38 | \node at (314.5pt,631.2383pt)[below right,color=black]{Kalman Integration with Drift and Motor Noise Filtering}; 39 | 40 | \draw[pstyle0] (437pt,475pt) rectangle (629.2069pt,507.7461pt); 41 | \node at (447pt,480pt)[below right,color=black]{Process Noise Adaptation}; 42 | 43 | \draw[pstyle0] (36pt,822pt) rectangle (492.0642pt,854.7461pt); 44 | \node at (46pt,827pt)[below right,color=black]{NMEA Data Stream (XDR Sentences)}; 45 | 46 | \draw[pstyle0] (353pt,724pt) rectangle (532.6658pt,756.7461pt); 47 | \node at (363pt,729pt)[below right,color=black]{MinMaxLemire}; 48 | 49 | \draw[pstyle0] (664pt,475pt) rectangle (764.4pt,507.7461pt); 50 | \node at (674pt,480pt)[below right,color=black]{De-Spike}; 51 | 52 | \draw[pstyle0] (568.5pt,724pt) rectangle (747.119pt,756.7461pt); 53 | \node at (578.5pt,729pt)[below right,color=black]{Wave Profile Predictor}; 54 | 55 | % --- Straight arrows instead of curves --- 56 | \draw[pstyle1,-latex] (444.5pt,45.36pt) -- (444.5pt,99.68pt); 57 | \node at (408.5pt,55.52pt)[below right,color=black]{accel}; 58 | 59 | \draw[pstyle1,-latex] (321.5pt,45.36pt) -- (321.5pt,99.68pt); 60 | \node at (286.5pt,55.52pt)[below right,color=black]{gyro}; 61 | 62 | \draw[pstyle1,-latex] (383pt,45.36pt) -- (383pt,99.68pt); 63 | \node at (344pt,55.52pt)[below right,color=black]{magn}; 64 | 65 | \draw[pstyle1,-latex] (383pt,143.13pt) -- (383pt,197.93pt); 66 | \node at (336pt,148.53pt)[below right]{${a_{z}}$}; 67 | 68 | \draw[pstyle1,-latex] (456.25pt,312.18pt) -- (456.25pt,366.82pt); 69 | \node at (393.25pt,317.5pt)[below right]{${f_{est}}$}; 70 | 71 | \draw[pstyle1,-latex] (569.25pt,143.2pt) -- (569.25pt,366.97pt); 72 | \node at (522.25pt,233.09pt)[below right]{${a_{z}}$}; 73 | 74 | \draw[pstyle1,-latex] (208.75pt,143.2pt) -- (208.75pt,366.97pt); 75 | \node at (105.75pt,233.09pt)[below right]{${a_{x}, a_{y}}$}; 76 | 77 | \draw[pstyle1,-latex] (309.75pt,312.18pt) -- (309.75pt,366.82pt); 78 | \node at (246.75pt,317.5pt)[below right]{${f_{est}}$}; 79 | 80 | \draw[pstyle1,-latex] (533pt,410.36pt) -- (533pt,464.68pt); 81 | \node at (398pt,415.52pt)[below right]{${R_{reg}, H_{est}}$}; 82 | 83 | \draw[pstyle1,-latex] (383pt,312.34pt) -- (383pt,562.93pt); 84 | \node at (320pt,415.64pt)[below right]{${f_{est}}$}; 85 | 86 | \draw[pstyle1,-latex] (533pt,508.18pt) -- (533pt,562.92pt); 87 | \node at (518pt,513.55pt)[below right]{${Q}$}; 88 | 89 | \draw[pstyle1,-latex] (698.75pt,143.11pt) -- (698.75pt,464.64pt); 90 | \node at (651.75pt,281.87pt)[below right]{${a_{z}}$}; 91 | 92 | \draw[pstyle1,-latex] (690.75pt,508.18pt) -- (690.75pt,562.92pt); 93 | \node at (643.75pt,513.55pt)[below right]{${a_{z}}$}; 94 | 95 | \draw[pstyle1,-latex] (443pt,659.15pt) -- (443pt,713.71pt); 96 | \node at (401pt,669.43pt)[below right,color=black]{heave}; 97 | 98 | \draw[pstyle1,-latex] (643pt,659.15pt) -- (643pt,713.71pt); 99 | \node at (601pt,669.43pt)[below right,color=black]{heave}; 100 | 101 | \draw[pstyle1,-latex] (216.25pt,410.12pt) -- (216.25pt,811.95pt); 102 | \node at (154.25pt,594.04pt)[below right,color=black]{angle, dir}; 103 | 104 | \draw[pstyle1,-latex] (422.5pt,757.36pt) -- (422.5pt,811.68pt); 105 | \node at (342.5pt,767.52pt)[below right,color=black]{wave height}; 106 | 107 | \draw[pstyle1,-latex] (328.75pt,659.06pt) -- (328.75pt,811.97pt); 108 | \node at (287pt,669.51pt)[below right,color=black]{heave}; 109 | \node at (285pt,685.99pt)[below right,color=black]{period}; 110 | \node at (286pt,702.47pt)[below right,color=black]{length}; 111 | \node at (285pt,718.95pt)[below right,color=black]{speed}; 112 | 113 | \draw[pstyle1,-latex] (82pt,143.04pt) -- (82pt,811.9pt); 114 | \node at (32.45pt,411.47pt)[below right,color=black]{roll}; 115 | \node at (26.68pt,427.95pt)[below right,color=black]{pitch}; 116 | \node at (29.47pt,444.43pt)[below right,color=black]{yaw}; 117 | \node at (7pt,460.91pt)[below right,color=black]{rate of turn}; 118 | 119 | \end{tikzpicture} 120 | }% end resizebox 121 | \end{center} 122 | 123 | \end{document} 124 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-ou12.tex-part: -------------------------------------------------------------------------------- 1 | \section*{Appendix A: Practical Assembly of the $12\times12$ OU Covariance (No Kronecker Needed)} 2 | 3 | This appendix gives a numerically robust recipe to assemble the translational 4 | process covariance $\mathbf{Q}_{LL}\in\mathbb{R}^{12\times12}$ for 5 | $\bigl[\mathbf{v},\mathbf{p},\mathbf{S},\mathbf{a}_w\bigr]$ without forming 6 | explicit Kronecker products. 7 | 8 | \subsection*{A.1 Inputs and Axis-Unit Covariance} 9 | \begin{itemize} 10 | \item Sampling step $h>0$, OU time constant $\tau>0$. 11 | \item Stationary covariance of the latent acceleration 12 | $\Sigma_{a_w}\in\mathbb{R}^{3\times3}$ (not necessarily diagonal). 13 | \item The per-axis discrete covariance 14 | $\mathbf{Q}^{(1)}_d(h,\tau,\sigma^2)\in\mathbb{R}^{4\times4}$ for 15 | the chain $[v,p,S,a]^\top$ driven by an OU process with 16 | stationary variance $\sigma^2$, obtained in closed form (with small 17 | $x=h/\tau$ series fallback). 18 | \end{itemize} 19 | Define the \emph{unit-variance} per-axis block (set $\sigma^2=1$) 20 | \[ 21 | \mathbf{Q}_{\text{axis}} \;\coloneqq\; \mathbf{Q}^{(1)}_d(h,\tau,1)\in\mathbb{R}^{4\times4}. 22 | \] 23 | Apply explicit symmetrization as hygiene: 24 | \( 25 | \mathbf{Q}_{\text{axis}} \leftarrow \tfrac{1}{2} 26 | (\mathbf{Q}_{\text{axis}}+\mathbf{Q}_{\text{axis}}^\top) 27 | \). 28 | 29 | \subsection*{A.2 Build the $12\times12$ Block Matrix in Axis-Packed Layout} 30 | Work first in the axis-packed order 31 | \[ 32 | [v_x,p_x,S_x,a_x \mid v_y,p_y,S_y,a_y \mid v_z,p_z,S_z,a_z], 33 | \] 34 | where the matrix naturally decomposes into $3\times3$ blocks of size $4\times4$. 35 | Scale each $4\times4$ block by the corresponding entry of the (symmetrized) 36 | acceleration covariance: 37 | \[ 38 | \mathbf{Q}_{\mathrm{axis\mbox{-}packed}}[i,j] \;=\; 39 | \bigl(\tfrac{1}{2}(\Sigma_{a_w}+\Sigma_{a_w}^\top)\bigr)_{ij}\;\mathbf{Q}_{\text{axis}}, 40 | \qquad i,j\in\{x,y,z\}. 41 | \] 42 | Concretely, 43 | \[ 44 | \mathbf{Q}_{\mathrm{axis\mbox{-}packed}} 45 | = 46 | \begin{bmatrix} 47 | \Sigma_{xx}\mathbf{Q}_{\text{axis}} & 48 | \Sigma_{xy}\mathbf{Q}_{\text{axis}} & 49 | \Sigma_{xz}\mathbf{Q}_{\text{axis}}\\[2pt] 50 | \Sigma_{yx}\mathbf{Q}_{\text{axis}} & 51 | \Sigma_{yy}\mathbf{Q}_{\text{axis}} & 52 | \Sigma_{yz}\mathbf{Q}_{\text{axis}}\\[2pt] 53 | \Sigma_{zx}\mathbf{Q}_{\text{axis}} & 54 | \Sigma_{zy}\mathbf{Q}_{\text{axis}} & 55 | \Sigma_{zz}\mathbf{Q}_{\text{axis}} 56 | \end{bmatrix}. 57 | \] 58 | This is algebraically equivalent to 59 | \( 60 | (\Sigma_{a_w}\otimes \mathbf{Q}_{\text{axis}}) 61 | \) 62 | but avoids forming the Kronecker product explicitly. 63 | 64 | \subsection*{A.3 Permute to State-Contiguous Layout} 65 | The filter’s state layout is grouped by \emph{state kind}, not by axis: 66 | \[ 67 | [\;v_x,v_y,v_z\;\mid\;p_x,p_y,p_z\;\mid\;S_x,S_y,S_z\;\mid\;a_x,a_y,a_z\;]. 68 | \] 69 | Define the permutation that maps axis-packed indices 70 | \( 71 | \{0{:}11\}=\{v_x,p_x,S_x,a_x,\ v_y,p_y,S_y,a_y,\ v_z,p_z,S_z,a_z\} 72 | \) 73 | to state-contiguous indices 74 | \( 75 | \{v_x,v_y,v_z,\ p_x,p_y,p_z,\ S_x,S_y,S_z,\ a_x,a_y,a_z\} 76 | \). 77 | One convenient mapping is 78 | \[ 79 | \pi = (0,4,8,\ 1,5,9,\ 2,6,10,\ 3,7,11), 80 | \] 81 | i.e. 82 | \[ 83 | \begin{aligned} 84 | &0\!\to\!0,\; 4\!\to\!1,\; 8\!\to\!2, &&\text{(all $v$)}\\ 85 | &1\!\to\!3,\; 5\!\to\!4,\; 9\!\to\!5, &&\text{(all $p$)}\\ 86 | &2\!\to\!6,\; 6\!\to\!7,\;10\!\to\!8, &&\text{(all $S$)}\\ 87 | &3\!\to\!9,\; 7\!\to\!10,\;11\!\to\!11. &&\text{(all $a$)} 88 | \end{aligned} 89 | \] 90 | Let $P_\pi$ be the $12\times12$ permutation matrix that reorders rows/cols by 91 | $\pi$. Then 92 | \[ 93 | \boxed{\; 94 | \mathbf{Q}_{LL} \;=\; P_\pi\,\mathbf{Q}_{\mathrm{axis\mbox{-}packed}}\,P_\pi^\top. 95 | \;} 96 | \] 97 | 98 | \subsection*{A.4 Final Hygiene: Symmetrize and PSD-Project} 99 | To counter roundoff and ensure numerical robustness: 100 | \[ 101 | \mathbf{Q}_{LL} \leftarrow \tfrac{1}{2}(\mathbf{Q}_{LL}+\mathbf{Q}_{LL}^\top). 102 | \] 103 | Optionally project to the nearest PSD matrix by eigenvalue flooring: 104 | if $\mathbf{Q}_{LL} = V\Lambda V^\top$ then 105 | \( 106 | \Lambda \leftarrow \max(\Lambda,\varepsilon I) 107 | \) 108 | with a tiny $\varepsilon$ (e.g.\ $10^{-16}$), and 109 | \( 110 | \mathbf{Q}_{LL}\leftarrow V\Lambda V^\top 111 | \). 112 | This mirrors the implementation’s guarded eigensolvers. 113 | 114 | \subsection*{A.5 Notes on Small $h/\tau$ and Stability} 115 | \begin{itemize} 116 | \item For $x=h/\tau\ll1$, compute $\mathbf{Q}_{\text{axis}}$ using 117 | Maclaurin series (up to $O(x^5)$) and \texttt{expm1}/\texttt{fma} 118 | combinations to avoid cancellation. 119 | \item Always sanitize inputs: 120 | symmetrize $\Sigma_{a_w}$, clamp its eigenvalues to a tiny positive 121 | floor before use, and guard $\tau$ away from zero. 122 | \item The procedure above is algebraically identical to 123 | \( 124 | (\Sigma_{a_w}^{1/2}\!\otimes I_4)\, 125 | (I_3\!\otimes \mathbf{Q}^{(1)}_d)\, 126 | (\Sigma_{a_w}^{1/2}\!\otimes I_4)^\top 127 | \), 128 | but avoids explicit square roots or Kronecker expansion. 129 | \end{itemize} 130 | 131 | \subsection*{A.6 Sanity Checks} 132 | \begin{enumerate} 133 | \item If $\Sigma_{a_w}=\sigma^2 I_3$, then 134 | $\mathbf{Q}_{LL}=\sigma^2\,\mathrm{blkdiag} 135 | (\mathbf{Q}_{\text{axis}},\mathbf{Q}_{\text{axis}},\mathbf{Q}_{\text{axis}})$ 136 | (up to the permutation). 137 | \item As $\tau\to\infty$ (very slow OU), $\mathbf{Q}_{\text{axis}}$ approaches 138 | the integrated white-noise covariance of a constant-acceleration model. 139 | \item As $\tau\to 0^+$, the OU reduces toward white acceleration; $\mathbf{Q}_{\text{axis}}$ 140 | tends to the standard integrated white process with step $h$. 141 | \end{enumerate} 142 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/Mahony_AHRS.h: -------------------------------------------------------------------------------- 1 | #ifndef Mahony_AHRS_h 2 | #define Mahony_AHRS_h 3 | 4 | // 5 | // Madgwick's implementation of Mahony's AHRS algorithm. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | 9 | #include 10 | 11 | #define RAD_TO_DEG 57.295779513082320876798154814105 12 | 13 | #define twoKpDef (2.0f * 1.0f) // 2 * proportional gain 14 | #define twoKiDef (2.0f * 0.0f) // 2 * integral gain 15 | 16 | typedef struct mahony_AHRS_vars { 17 | float twoKp = twoKpDef; // 2 * proportional gain (Kp) 18 | float twoKi = twoKiDef; // 2 * integral gain (Ki) 19 | float q0 = 1.0; 20 | float q1 = 0.0; 21 | float q2 = 0.0; 22 | float q3 = 0.0; // quaternion of sensor frame relative to auxiliary frame 23 | float integralFBx = 0.0f; 24 | float integralFBy = 0.0f; 25 | float integralFBz = 0.0f; // integral error terms scaled by Ki 26 | } Mahony_AHRS_Vars; 27 | 28 | void mahony_AHRS_init(Mahony_AHRS_Vars* m, float twoKp, float twoKi); 29 | 30 | void mahony_AHRS_update(Mahony_AHRS_Vars* m, 31 | float gx, float gy, float gz, float ax, float ay, float az, 32 | float *pitch, float *roll, float *yaw, float delta_t_sec); 33 | float invSqrt(float x); 34 | 35 | /* 36 | The gain is the Kp term in a PID controller, tune it as you would any PID controller (missing the I and D terms). 37 | If Kp is too low, the filter will respond slowly to changes in sensor orientation. 38 | If too high, the filter output will oscillate. 39 | */ 40 | void mahony_AHRS_init(Mahony_AHRS_Vars* m, float twoKp, float twoKi) { 41 | m->twoKp = twoKp; 42 | m->twoKi = twoKi; 43 | } 44 | 45 | // IMU algorithm update (without magnetometer) 46 | void mahony_AHRS_update(Mahony_AHRS_Vars* m, 47 | float gx, float gy, float gz, float ax, float ay, float az, 48 | float *pitch, float *roll, float *yaw, float delta_t_sec) { 49 | float recipNorm; 50 | float halfvx, halfvy, halfvz; 51 | float halfex, halfey, halfez; 52 | float qa, qb, qc; 53 | 54 | // Compute feedback only if accelerometer measurement valid (avoids NaN in 55 | // accelerometer normalisation) 56 | if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 57 | // Normalise accelerometer measurement 58 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 59 | ax *= recipNorm; 60 | ay *= recipNorm; 61 | az *= recipNorm; 62 | 63 | // Estimated direction of gravity and vector perpendicular to magnetic flux 64 | halfvx = m->q1 * m->q3 - m->q0 * m->q2; 65 | halfvy = m->q0 * m->q1 + m->q2 * m->q3; 66 | halfvz = m->q0 * m->q0 - 0.5f + m->q3 * m->q3; 67 | 68 | // Error is sum of cross product between estimated and measured direction of gravity 69 | halfex = (ay * halfvz - az * halfvy); 70 | halfey = (az * halfvx - ax * halfvz); 71 | halfez = (ax * halfvy - ay * halfvx); 72 | 73 | // Compute and apply integral feedback if enabled 74 | if (m->twoKi > 0.0f) { 75 | m->integralFBx += m->twoKi * halfex * delta_t_sec; // integral error scaled by Ki 76 | m->integralFBy += m->twoKi * halfey * delta_t_sec; 77 | m->integralFBz += m->twoKi * halfez * delta_t_sec; 78 | gx += m->integralFBx; // apply integral feedback 79 | gy += m->integralFBy; 80 | gz += m->integralFBz; 81 | } else { 82 | m->integralFBx = 0.0f; // prevent integral windup 83 | m->integralFBy = 0.0f; 84 | m->integralFBz = 0.0f; 85 | } 86 | 87 | // Apply proportional feedback 88 | gx += m->twoKp * halfex; 89 | gy += m->twoKp * halfey; 90 | gz += m->twoKp * halfez; 91 | } 92 | 93 | // Integrate rate of change of quaternion 94 | gx *= (0.5f * delta_t_sec); // pre-multiply common factors 95 | gy *= (0.5f * delta_t_sec); 96 | gz *= (0.5f * delta_t_sec); 97 | qa = m->q0; 98 | qb = m->q1; 99 | qc = m->q2; 100 | m->q0 += (-qb * gx - qc * gy - m->q3 * gz); 101 | m->q1 += (qa * gx + qc * gz - m->q3 * gy); 102 | m->q2 += (qa * gy - qb * gz + m->q3 * gx); 103 | m->q3 += (qa * gz + qb * gy - qc * gx); 104 | 105 | // Normalise quaternion 106 | recipNorm = invSqrt(m->q0 * m->q0 + m->q1 * m->q1 + m->q2 * m->q2 + m->q3 * m->q3); 107 | m->q0 *= recipNorm; 108 | m->q1 *= recipNorm; 109 | m->q2 *= recipNorm; 110 | m->q3 *= recipNorm; 111 | 112 | *pitch = asin(-2 * m->q1 * m->q3 + 2 * m->q0 * m->q2); // pitch 113 | *roll = atan2(2 * m->q2 * m->q3 + 2 * m->q0 * m->q1, 114 | -2 * m->q1 * m->q1 - 2 * m->q2 * m->q2 + 1); // roll 115 | *yaw = atan2(2 * (m->q1 * m->q2 + m->q0 * m->q3), 116 | m->q0 * m->q0 + m->q1 * m->q1 - m->q2 * m->q2 - m->q3 * m->q3); // yaw 117 | 118 | *pitch *= RAD_TO_DEG; 119 | *yaw *= RAD_TO_DEG; 120 | *roll *= RAD_TO_DEG; 121 | } 122 | 123 | /** 124 | * Fast inverse square root implementation. Compatible both for 32 and 8 bit microcontrollers. 125 | * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root 126 | */ 127 | #pragma GCC diagnostic push 128 | #pragma GCC diagnostic ignored "-Wstrict-aliasing" 129 | 130 | float invSqrt(float number) { 131 | union { 132 | float f; 133 | int32_t i; 134 | } y; 135 | 136 | y.f = number; 137 | y.i = 0x5f375a86 - (y.i >> 1); 138 | y.f = y.f * ( 1.5f - ( number * 0.5f * y.f * y.f ) ); 139 | return y.f; 140 | } 141 | 142 | /* Old 8bit version. Kept it here only for testing/debugging of the new code above. 143 | float invSqrt(float number) { 144 | volatile long i; 145 | volatile float x, y; 146 | volatile const float f = 1.5F; 147 | 148 | x = number * 0.5F; 149 | y = number; 150 | i = * ( long * ) &y; 151 | i = 0x5f375a86 - ( i >> 1 ); 152 | y = * ( float * ) &i; 153 | y = y * ( f - ( x * y * y ) ); 154 | return y; 155 | } 156 | */ 157 | 158 | #pragma GCC diagnostic pop 159 | 160 | #endif 161 | 162 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/w3d-proc-symb-assembly.tex-part: -------------------------------------------------------------------------------- 1 | \subsection{Final Discrete Assembly \texorpdfstring{$(\Phi,Q_d)$}{(Phi,Qd)} for the Full Process Model} 2 | 3 | Let the discrete state at step \(k\) be ordered as 4 | \[ 5 | \boldsymbol{x}_k \;=\; 6 | \begin{bmatrix} 7 | \delta\boldsymbol{\theta} & 8 | \mathbf{b}_g & 9 | \mathbf{v} & 10 | \mathbf{p} & 11 | \mathbf{S} & 12 | \mathbf{a}_w & 13 | \mathbf{b}_a 14 | \end{bmatrix}^{\!\top} 15 | \in\mathbb{R}^{21},\\ 16 | \] 17 | where \(\delta\boldsymbol{\theta}\in\mathbb{R}^3\) is the (right–multiplicative) attitude error, 18 | \(\mathbf{b}_g\in\mathbb{R}^3\) the gyro bias, 19 | \((\mathbf{v},\mathbf{p},\mathbf{S},\mathbf{a}_w)\in\mathbb{R}^{12}\) the linear OU–driven chain 20 | stacked as \([v(3),p(3),S(3),a_w(3)]\), and \(\mathbf{b}_a\in\mathbb{R}^3\) the accelerometer bias. 21 | 22 | \subsubsection*{Block transition matrix \(\Phi\)} 23 | For a sample period \(h>0\), the full transition matrix is block upper–triangular: 24 | \[ 25 | \begin{aligned} 26 | \Phi(h)&= 27 | \begin{bmatrix} 28 | \Phi_{AA}(h) & 0 & 0 \\ 29 | 0 & \Phi_{LL}(h) & 0 \\ 30 | 0 & 0 & \Phi_{BB}(h) 31 | \end{bmatrix},\\ 32 | &\quad 33 | \text{with blocks indexed by } 34 | \underbrace{A}_{(\delta\boldsymbol{\theta},\mathbf{b}_g)},\; 35 | \underbrace{L}_{(\mathbf{v},\mathbf{p},\mathbf{S},\mathbf{a}_w)},\; 36 | \underbrace{B}_{(\mathbf{b}_a)} 37 | \end{aligned}. 38 | \] 39 | \paragraph{Attitude\(+\)gyro–bias block \(\Phi_{AA}(h)\in\mathbb{R}^{6\times 6}\).} 40 | Using the standard Q–MEKF constant–rate approximation over \([t_k,t_k+h]\), the 41 | attitude–error subblock implements the right–multiplicative discrete kinematics, 42 | and the bias is random–walk with identity transition: 43 | \[ 44 | \Phi_{AA}(h)= 45 | \begin{bmatrix} 46 | \Phi_{\theta\theta}(h) & \Phi_{\theta b_g}(h)\\[2pt] 47 | 0 & I_3 48 | \end{bmatrix}, 49 | \] 50 | where \(\Phi_{\theta\theta}(h)\) is the \(3\times3\) small–angle propagation induced by the 51 | body rate (constant over the step), and \(\Phi_{\theta b_g}(h)\) captures the 52 | first–order coupling from \(b_g\) into attitude error over the step.\footnote{Either 53 | the exact closed–form structured integral (rotation–integral path) or the fast 54 | \(\Phi\approx I - [\omega]_\times h\) with \(\Phi_{\theta b_g}\approx -I\,h\).} 55 | 56 | \paragraph{Linear OU–driven block \(\Phi_{LL}(h)\in\mathbb{R}^{12\times 12}\).} 57 | Let \(\Phi_{\text{axis}}(h)\in\mathbb{R}^{4\times4}\) denote the per–axis transition for 58 | \([v,p,S,a]^\top\) (Section~\emph{OU analytic discretization}); then, with one shared 59 | \(\tau\), 60 | \[ 61 | \begin{aligned} 62 | \Phi_{LL}(h)&=I_3\;\otimes\;\Phi_{\text{axis}}(h), 63 | \quad\\ 64 | \Phi_{\text{axis}}(h)&= 65 | \begin{bmatrix} 66 | 1 & 0 & 0 & \phi_{va} \\ 67 | h & 1 & 0 & \phi_{pa} \\ 68 | \frac{h^2}{2} & h & 1 & \phi_{Sa} \\ 69 | 0 & 0 & 0 & \alpha 70 | \end{bmatrix}, 71 | \qquad 72 | \alpha=e^{-h/\tau} 73 | \end{aligned}, 74 | \] 75 | with feedthroughs 76 | \(\phi_{va}=-\tau(\alpha-1)\), 77 | \(\phi_{pa}=\tau^2\bigl(\tfrac{h}{\tau}+(\alpha-1)\bigr)\), 78 | \(\phi_{Sa}=\tau^3\bigl(\tfrac{1}{2}(\tfrac{h}{\tau})^2-\tfrac{h}{\tau}-(\alpha-1)\bigr)\). 79 | 80 | \paragraph{Accelerometer–bias block \(\Phi_{BB}(h)\in\mathbb{R}^{3\times 3}\).} 81 | Random–walk bias: 82 | \[ 83 | \Phi_{BB}(h)=I_3. 84 | \] 85 | 86 | \subsubsection*{Block process covariance \(Q_d\)} 87 | With mutually independent driving noises (gyro RW, OU forcing, accel–bias RW), 88 | the discrete process covariance is block diagonal: 89 | \[ 90 | Q_d(h)= 91 | \begin{bmatrix} 92 | Q_{AA}(h) & 0 & 0 \\ 93 | 0 & Q_{LL}(h) & 0 \\ 94 | 0 & 0 & Q_{BB}(h) 95 | \end{bmatrix}. 96 | \] 97 | 98 | \paragraph{Attitude\(+\)gyro–bias block \(Q_{AA}(h)\in\mathbb{R}^{6\times 6}\).} 99 | Structured discrete covariance for \((\delta\boldsymbol{\theta},\mathbf{b}_g)\) over the step, 100 | assembled from the continuous gyro–noise and bias–RW intensities via the rotation–integral 101 | closed forms (or the fast \(Q\approx Q_{\text{cont}}\,h\) path): 102 | \[ 103 | Q_{AA}(h)= 104 | \begin{bmatrix} 105 | Q_{\theta\theta}(h) & Q_{\theta b_g}(h)\\[2pt] 106 | Q_{\theta b_g}(h)^{\!\top} & Q_{b_gb_g}(h) 107 | \end{bmatrix}. 108 | \] 109 | 110 | \paragraph{Linear OU–driven block \(Q_{LL}(h)\in\mathbb{R}^{12\times 12}\).} 111 | Let \(\Sigma_{aw}\in\mathbb{R}^{3\times3}\) be the SPD stationary covariance of the latent 112 | world acceleration (potentially anisotropic and correlated), common \(\tau\). With the 113 | unit kernel \(K(h,\tau)\in\mathbb{R}^{4\times4}\) from the one–axis derivation and 114 | \(\mathrm{sym}\,K=\tfrac12(K+K^\top)\), 115 | \[ 116 | Q_{LL}(h)\;=\;\frac{2}{\tau}\;\Sigma_{aw}\;\otimes\;\mathrm{sym}\,K(h,\tau). 117 | \] 118 | (If \(\Sigma_{aw}=\mathrm{diag}(s_x^2,s_y^2,s_z^2)\), this yields a block–diagonal 119 | \(Q_{LL}\) with three \(4\times4\) OU covariances scaled by \(s_x^2,s_y^2,s_z^2\).) 120 | 121 | \paragraph{Accelerometer–bias block \(Q_{BB}(h)\in\mathbb{R}^{3\times 3}\).} 122 | Random–walk discretization: 123 | \[ 124 | Q_{BB}(h)=Q_{b_a}\,h, 125 | \] 126 | where \(Q_{b_a}\) is the continuous RW intensity (diagonal typically), optionally 127 | augmented by temperature–drift modeling in the measurement equation. 128 | 129 | \subsubsection*{Prediction step (context)} 130 | With these blocks, 131 | \[ 132 | \begin{aligned} 133 | \hat{\boldsymbol{x}}_{k|k-1}&=\Phi(h)\,\hat{\boldsymbol{x}}_{k-1|k-1},\qquad\\ 134 | P_{k|k-1}&=\Phi(h)\,P_{k-1|k-1}\,\Phi(h)^{\!\top}+Q_d(h) 135 | \end{aligned}. 136 | \] 137 | The attitude/bias block \((\Phi_{AA},Q_{AA})\) follows the multiplicative EKF 138 | constant–rate assumption over \([t_k,t_k+h]\); the linear block \((\Phi_{LL},Q_{LL})\) uses 139 | the analytic OU discretization; the accel–bias block is identity with RW covariance. 140 | All measurement updates are applied in Joseph form with PSD projection for numerical stability with floating-point operations. 141 | -------------------------------------------------------------------------------- /bbn_wave_freq_m5atomS3/doc/freq_tracking_adaptive_notch_kalman.tex: -------------------------------------------------------------------------------- 1 | \documentclass[11pt,letterpaper]{article} 2 | \usepackage{amsmath} 3 | \usepackage{amssymb} 4 | \usepackage{mathrsfs} 5 | \usepackage{fullpage} 6 | \usepackage{graphicx} 7 | \usepackage{hyperref} 8 | \usepackage{cite} 9 | 10 | \title{Comprehensive Analysis of a Kalman-Based Adaptive Notch Filter Frequency Tracker} 11 | \author{} 12 | \date{} 13 | 14 | \begin{document} 15 | 16 | \maketitle 17 | 18 | \section{Introduction} 19 | This work presents a frequency tracking algorithm based on a Kalman-updated adaptive notch filter, following the approach of Ali and van Waterschoot~\cite{AliWaterschoot2023}. The method estimates the instantaneous frequency of a noisy sinusoidal signal through recursive adaptation of a single parameter. 20 | 21 | \section{Signal Model and Notch Filter} 22 | \subsection{Signal Model} 23 | The measured signal is assumed to be: 24 | \begin{equation} 25 | y(t) = A(t)\,\sin\bigl(\omega(t)\,t + \phi(t)\bigr) + \nu(t), 26 | \label{eq:signal_model} 27 | \end{equation} 28 | where $A(t)$, $\phi(t)$, and $\omega(t)$ vary slowly, and $\nu(t)$ is zero-mean noise. 29 | 30 | \subsection{Notch Filter} 31 | A notch filter is a type of linear time-invariant (LTI) filter designed to attenuate or remove a specific frequency (and a very narrow band around it) from a signal while leaving other frequencies largely unaffected. It is characterized by its frequency response, which has a sharp dip (a "notch") at the target frequency. 32 | 33 | \subsection{Notch Filter Structure} 34 | The second-order digital notch filter has transfer function: 35 | \begin{equation} 36 | H(z^{-1};a) = \frac{1 - a\,z^{-1} + z^{-2}}{1 - \rho\,a\,z^{-1} + \rho^2\,z^{-2}}, 37 | \label{eq:notch_tf} 38 | \end{equation} 39 | where $a = 2\cos(\omega \Delta t)$ and $\rho \in (0,1)$ is the pole radius. The instantaneous frequency is recovered as: 40 | \begin{equation} 41 | \omega = \frac{1}{\Delta t}\arccos\!\left(\frac{a}{2}\right). 42 | \label{eq:freq_estimate} 43 | \end{equation} 44 | 45 | \section{Continuous-Time Interpretation} 46 | \subsection{Resonator Dynamics} 47 | The resonator state $s(t)$ follows: 48 | \begin{equation} 49 | \ddot{s}(t) + 2\zeta(a)\omega(a)\dot{s}(t) + \omega(a)^2 s(t) = y(t), 50 | \label{eq:resonator_dynamics} 51 | \end{equation} 52 | where: 53 | \begin{align} 54 | \omega(a) &= \frac{1}{\Delta t}\cos^{-1}\!\left(\frac{a}{2}\right), \label{eq:omega_def} \\ 55 | \zeta(a) &= -\frac{\ln\rho}{\sqrt{\pi^2 + (\ln\rho)^2}}. \label{eq:zeta_def} 56 | \end{align} 57 | 58 | \subsection{Energy Interpretation} 59 | The resonator's energy evolves as: 60 | \begin{equation} 61 | \dot{E}(t) = \dot{s}(t)y(t) - 2\zeta\omega \dot{s}(t)^2, 62 | \label{eq:energy} 63 | \end{equation} 64 | showing energy transfer from input to resonator at the resonant frequency. 65 | 66 | \section{Discrete-Time Algorithm} 67 | \subsection{State-Space Update} 68 | \begin{enumerate} 69 | \item \textbf{State Prediction}: 70 | \begin{equation} 71 | s[n] = y[n] + \rho a[n-1]s[n-1] - \rho^2 s[n-2]. 72 | \label{eq:state_update} 73 | \end{equation} 74 | 75 | \item \textbf{Innovation}: 76 | \begin{equation} 77 | e[n] = s[n] - \rho a[n-1]s[n-1] + \rho^2 s[n-2]. 78 | \label{eq:innovation} 79 | \end{equation} 80 | 81 | \item \textbf{Kalman Update}: 82 | \begin{align} 83 | K[n] &= \frac{\rho s[n-1] P_{n|n-1}}{(\rho s[n-1])^2 P_{n|n-1} + r}, \label{eq:kalman_gain} \\ 84 | a[n] &= a[n-1] + K[n]e[n], \label{eq:param_update} \\ 85 | P_n &= (1 - K[n]\rho s[n-1]) P_{n|n-1}. \label{eq:cov_update} 86 | \end{align} 87 | 88 | \item \textbf{Frequency Estimate}: 89 | \begin{equation} 90 | \hat{\omega}[n] = \frac{1}{\Delta t}\arccos\!\left(\frac{a[n]}{2}\right). 91 | \label{eq:freq_update} 92 | \end{equation} 93 | \end{enumerate} 94 | 95 | \subsection{Parameter Selection} 96 | \begin{itemize} 97 | \item Pole radius: $\rho \approx 0.99$ (narrow notch) to $0.999$ (high resolution) 98 | \item Initial covariance: $P_0 = 10^2$ for uncertain initial $a[0]$ 99 | \end{itemize} 100 | 101 | \section{Stability and Convergence} 102 | \subsection{Steady-State Behavior} 103 | The error covariance converges to: 104 | \begin{equation} 105 | \mathbb{E}\bigl[(a[n] - a_{\text{true}})^2\bigr] \to \frac{r}{s[n-1]^2 + r/q}. 106 | \label{eq:steady_state} 107 | \end{equation} 108 | 109 | \subsection{Numerical Stability} 110 | \begin{itemize} 111 | \item Clamp $-2 < a[n] < 2$ to avoid $\arccos$ domain errors 112 | \item Maintain $0 < P[n] < P_{\text{max}}$ to prevent divergence 113 | \end{itemize} 114 | 115 | \section{Performance and Limitations} 116 | \subsection{Frequency Resolution} 117 | The bandwidth is approximately: 118 | \begin{equation} 119 | BW \approx \frac{1 - \rho}{\pi \Delta t}. 120 | \label{eq:bandwidth} 121 | \end{equation} 122 | 123 | \subsection{Noise Robustness} 124 | \begin{itemize} 125 | \item Optimal for zero-mean white noise 126 | \item Requires DC removal for non-zero mean signals 127 | \end{itemize} 128 | 129 | \section{Extended Kalman Filter Interpretation} 130 | The algorithm implements a scalar EKF with Jacobian: 131 | \begin{equation} 132 | H_a(t) = \frac{\partial h(s,a)}{\partial a} = \rho s(t). 133 | \label{eq:jacobian} 134 | \end{equation} 135 | 136 | \section{Numerical Considerations} 137 | \begin{itemize} 138 | \item Use $\rho \leq 0.99$ for stability 139 | \item Double precision recommended when $\rho > 0.99$ 140 | \end{itemize} 141 | 142 | \section{Conclusion} 143 | The Kalman-based adaptive notch filter provides computationally efficient frequency tracking with inherent noise robustness. The scalar EKF structure ensures stability while maintaining adaptation capability. 144 | 145 | \begin{thebibliography}{9} 146 | \bibitem{AliWaterschoot2023} 147 | R. Ali and T. van Waterschoot, 148 | ``A frequency tracker based on a Kalman filter update of a single parameter adaptive notch filter,'' 149 | in \emph{Proc. 26th Int. Conf. Digital Audio Effects (DAFx23)}, 150 | Copenhagen, Denmark, Sep. 2023. 151 | \end{thebibliography} 152 | 153 | \end{document} 154 | --------------------------------------------------------------------------------