sysid implementation

.coveragerc

@@ -6,6 +6,8 @@ omit =
     */__init__.py
     */src/data/accel/accelerometer.py
     */src/data/accel/aligner.py
+    */src/functions/natural_freq.py
+    */src/methods/pyoma/*
     */src/examples/*
 
 [report]

.pylintrc

@@ -24,7 +24,7 @@ ignore=CVS
 
 # Add files or directories matching the regex patterns to the ignore-list. The
 # regex matches against paths and can be in Posix or Windows format.
-ignore-paths=
+ignore-paths=src/methods/pyoma
 
 # Files or directories matching the regex patterns are skipped. The regex
 # matches against base names, not paths. The default value ignores emacs file

config/production.json.template

@@ -7,12 +7,21 @@
         "ClientID": "test_client_id",
         "QoS": 1,
         "TopicsToSubscribe": [
-            "cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/1/acc/raw/data",
-            "cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/1/acc/raw/metadata",  
-            "cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/2/acc/raw/data",
-            "cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/2/acc/raw/metadata",
-            "cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/3/acc/raw/data",
-            "cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/3/acc/raw/metadata"
+        "cpsens/d8-3a-dd-37-d2-7e/3160-A-042_sn_999998/1/acc/raw/data",
+        "cpsens/d8-3a-dd-37-d2-7e/3160-A-042_sn_999998/1/acc/raw/metadata",
+        "cpsens/d8-3a-dd-37-d2-7e/3160-A-042_sn_999998/2/acc/raw/data",
+        "cpsens/+/+/1/acc/raw/data",
+        "cpsens/+/+/2/acc/raw/data"
         ]
+    },
+
+    "sysID": {
+      "host": "",
+      "port": 1883,
+      "userId": "",
+      "password": "",
+      "ClientID": "sub.232.sds.213s",
+      "QoS": 1,
+      "TopicsToSubscribe": ["cpsens/d8-3a-dd-f5-92-48/cpsns_Simulator/1_2/oma_results"]
     }
 }

pyproject.toml

@@ -13,8 +13,10 @@ packages = [{include = "*", from="src"}]
 [tool.poetry.dependencies]
 python = ">=3.12, <3.13"
 paho-mqtt = "^2.1.0"
-numpy = "^2.2.4"
+numpy = "^2.2.5"
 click ="^8.1.8"
+pyOMA-2 = "1.0.0"
+
 
 [tool.poetry.group.dev.dependencies]
 pylint = "^3.3.6"

src/data/accel/constants.py

@@ -1,7 +1,9 @@
-MAX_MAP_SIZE = 52200  # The maximum number of samples saved in FIFO
+MAX_MAP_SIZE = 160100  # The maximum number of samples saved in FIFO
 
 TIMEOUT = 2  # Max wait time until enough samples are collected for the test_Accelerometer
 
 INTERVAL = 0.001  # Check every 0.001s to see if samples are collected
 
 MIN_SAMPLES_NEEDED = 500  # Minimum samples needed before running it to sysid
+
+WAIT_METADATA = 11 # Wait max 11 seconds for getting metadata message

src/data/accel/hbk/accelerometer.py

@@ -84,7 +84,7 @@ def process_message(self, msg: mqtt.MQTTMessage) -> None:
                     if not oldest_deque:  # Remove the key/deque from the map if it's empty
                         del self.data_map[oldest_key]
                     total_samples = sum(len(dq) for dq in self.data_map.values())
-            print(f" Channel: {self.topic}  Key: {samples_from_daq_start}, Samples: {num_samples}")
+            #print(f" Channel: {self.topic}  Key: {samples_from_daq_start}, Samples: {num_samples}")
 
         except Exception as e:
             print(f"Error processing message: {e}")

src/data/accel/hbk/aligner.py

@@ -111,9 +111,15 @@ def _extract_aligned_block(self, group: List[int], batch_size: int,
             for i in range(batch_size):
                 if samples_collected >= requested_samples:
                     break
+                # Iterate over each channel index and
+                # its corresponding data entries for the current key
                 for ch_idx, channel_data in enumerate(entries):
-                    if channel_data is not None:
-                        aligned_data[ch_idx].append(channel_data[i])
+                    # Get the i-th sample from the channel, if it exists
+                    sample = channel_data[i] if channel_data and i < len(channel_data) else None
+                    # If a valid sample was retrieved,
+                    # append it to the aligned data for that channel
+                    if sample is not None:
+                        aligned_data[ch_idx].append(sample)
                     else:
                         print(f"Missing data for channel index {ch_idx} skipping")
                 samples_collected += 1
@@ -131,7 +137,7 @@ def _extract_aligned_block(self, group: List[int], batch_size: int,
     def extract(self, requested_samples: int) -> Tuple[np.ndarray, Optional[datetime]]:
         with self._lock:
             batch_size, key_groups = self.find_continuous_key_groups()
-            print("Keys", key_groups)
+            #print("Keys", key_groups)
 
             if batch_size is None or key_groups is None:
                 # No data or groups to align, returun empty

src/data/accel/metadata.py

@@ -0,0 +1,37 @@
+import json
+import time
+from typing import Any, Dict
+from paho.mqtt.client import Client as MQTTClient
+from data.accel.constants import WAIT_METADATA
+from data.comm.mqtt import setup_mqtt_client
+
+def extract_fs_from_metadata(mqtt_config: Dict[str, Any]) -> int:
+    fs_result = {"fs": None}
+
+    def _on_metadata(client: MQTTClient, userdata, message) -> None:
+        try:
+            payload = json.loads(message.payload.decode("utf-8"))
+            fs_candidate = payload["Analysis chain"][0]["Sampling"]
+            if fs_candidate:
+                fs_result["fs"] = fs_candidate
+                print(f"Extracted Fs from metadata: {fs_candidate}")
+                client.unsubscribe(userdata["metadata_topic"])
+        except Exception as e:
+            print(f"Failed to extract Fs: {e}")
+
+    metadata_topic = mqtt_config["TopicsToSubscribe"][1]
+    client, _ = setup_mqtt_client(mqtt_config, topic_index=1)
+    client.user_data_set({"metadata_topic": metadata_topic})
+    client.message_callback_add(metadata_topic, _on_metadata)
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+    client.subscribe(metadata_topic)
+    client.loop_start()
+
+    start_time = time.time()
+    while fs_result["fs"] is None and (time.time() - start_time) < WAIT_METADATA:
+        time.sleep(0.1)
+
+    client.loop_stop()
+    if fs_result["fs"] is None:
+        raise TimeoutError("Sampling frequency not received within timeout")
+    return fs_result["fs"]

src/examples/README.md

@@ -42,11 +42,21 @@ There are two examples.
 * **example_2** demonstrates the use of `Aligner` class to collect and
   align accelerometer measurements from multiple MQTT data streams.
 
+* **example_3** demonstrates the use of `sys_id` with 3 cases: 
+    1. run_experiment_3_plot: plots natural frequencies.
+    2. run_experiment_3_print: prints OMA results to console.
+    3. run_experiment_3_publish: publishes OMA results via MQTT to the config given under [sysid] config.
+
 To run the examples with the default config, use:
 
 ```bash
 python .\src\examples\example.py experiment-1
 python .\src\examples\example.py experiment-2
+python .\src\examples\example.py experiment-3-print
+python .\src\examples\example.py experiment-3-plot
+python .\src\examples\example.py experiment-3-publish
+
+
 ```
 
 To run the examples with specified config, use

src/examples/example.py

@@ -2,9 +2,15 @@
 import click
 from examples.experiment_1 import run_experiment_1
 from examples.experiment_2 import run_experiment_2
+from examples.experiment_3 import (
+    run_experiment_3_plot,
+    run_experiment_3_publish,
+    run_experiment_3_print,
+)
+
 
 @click.group()
-@click.option('--config', default="config/mockpt.json", help="Path to config file")
+@click.option('--config', default="config/production.json", help="Path to config file")
 @click.pass_context
 def cli(ctx, config):
     ctx.ensure_object(dict)
@@ -20,5 +26,21 @@ def experiment_1(ctx):
 def experiment_2(ctx):
     run_experiment_2(ctx.obj["CONFIG"])
 
+
+@cli.command()
+@click.pass_context
+def experiment_3_publish(ctx):
+    run_experiment_3_publish(ctx.obj["CONFIG"])
+
+@cli.command()
+@click.pass_context
+def experiment_3_plot(ctx):
+    run_experiment_3_plot(ctx.obj["CONFIG"])
+
+@cli.command()
+@click.pass_context
+def experiment_3_print(ctx):
+    run_experiment_3_print(ctx.obj["CONFIG"])
+
 if __name__ == "__main__":
     cli(obj={})

src/examples/experiment_2.py

@@ -7,7 +7,7 @@
 def run_experiment_2(config_path):
     config = load_config(config_path)
     mqtt_config = config["MQTT"]
-    topic_indexes = [0,1]
+    topic_indexes = [0,2]
 
     all_topics = mqtt_config["TopicsToSubscribe"]
     selected_topics = [all_topics[i] for i in topic_indexes]

src/examples/experiment_3.py

@@ -0,0 +1,83 @@
+import sys
+import matplotlib.pyplot as plt
+from methods import sys_id as sysID
+from data.comm.mqtt import load_config
+from data.accel.hbk.aligner import Aligner
+from functions.natural_freq import plot_natural_frequencies
+
+
+def run_experiment_3_plot(config_path):
+    number_of_minutes = 0.2
+    config = load_config(config_path)
+    mqtt_config = config["MQTT"]
+
+    # Setting up the client and extracting Fs
+    data_client, fs = sysID.setup_client(mqtt_config)
+
+    # Setting up the aligner
+    data_topic_indexes = [0, 2]
+    selected_topics = [mqtt_config["TopicsToSubscribe"][i] for i in data_topic_indexes]
+    aligner = Aligner(data_client, topics=selected_topics)
+
+    fig_ax = None
+    aligner_time = None
+    while aligner_time is None:
+        results, aligner_time = sysID.get_oma_results(number_of_minutes, aligner, fs)
+    data_client.disconnect()
+    fig_ax = plot_natural_frequencies(results['Fn_poles'], freqlim=(0, 75), fig_ax=fig_ax)
+    plt.show(block=True)
+    sys.stdout.flush()
+
+
+def run_experiment_3_print(config_path):
+    number_of_minutes = 0.2
+    config = load_config(config_path)
+    mqtt_config = config["MQTT"]
+
+    # Setting up the client and extracting Fs
+    data_client, fs = sysID.setup_client(mqtt_config)
+
+    # Setting up the aligner
+    data_topic_indexes = [0, 2]
+    selected_topics = [mqtt_config["TopicsToSubscribe"][i] for i in data_topic_indexes]
+    aligner = Aligner(data_client, topics=selected_topics)
+
+    aligner_time = None
+    while aligner_time is None:
+        results, aligner_time = sysID.get_oma_results(number_of_minutes, aligner, fs)
+    data_client.disconnect()
+    sys.stdout.flush()
+
+    print(f"\n System Frequencies \n {results['Fn_poles']}")
+    print(f"\n Cov \n{results['Fn_poles_cov']}")
+    print(f"\n damping_ratios  \n{results['Xi_poles']}")
+    print(f"\n cov_damping \n{results['Xi_poles_cov']}")
+
+
+def run_experiment_3_publish(config_path):
+    number_of_minutes = 0.02
+    config = load_config(config_path)
+    mqtt_config = config["MQTT"]
+    publish_config = config["sysID"]
+
+    # Setting up the client for getting accelerometer data
+    data_client, fs = sysID.setup_client(mqtt_config)
+
+    # Setting up the aligner
+    data_topic_indexes = [0, 2]
+    selected_topics = [mqtt_config["TopicsToSubscribe"][i] for i in data_topic_indexes]
+    aligner = Aligner(data_client, topics=selected_topics)
+
+    # Setting up the client for publishing OMA results
+    publish_client, _ = sysID.setup_client(publish_config)  # fs not needed here
+
+    sysID.publish_oma_results(
+        number_of_minutes,
+        aligner,
+        publish_client,
+        publish_config["TopicsToSubscribe"][0],
+        fs
+    )
+
+    print(f"Publishing to topic: {publish_config['TopicsToSubscribe'][0]}")
+    sys.stdout.flush()

src/functions/natural_freq.py

@@ -0,0 +1,59 @@
+from typing import Optional, Tuple
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import ticker
+
+
+def plot_natural_frequencies(
+    fn_poles: np.ndarray,
+    freqlim: Optional[Tuple[float, float]] = None,
+    fig_ax: Optional[Tuple] = None
+) -> Tuple:
+    """
+    Plots a stabilization diagram with optional frequency range filtering.
+
+    Args:
+        Fn_poles (np.ndarray): 2D array [model_order x mode] of natural frequencies.
+        freqlim (tuple, optional): (f_min, f_max) frequency range to plot.
+        fig_ax (tuple, optional): Reuse (fig, ax) for interactive updates.
+
+    Returns:
+        (fig, ax): The matplotlib figure and axes used for plotting.
+    """
+    if fig_ax is None:
+        plt.ion()
+        fig, ax = plt.subplots(figsize=(8, 5))
+    else:
+        fig, ax = fig_ax
+        ax.clear()
+
+    num_orders, num_modes = fn_poles.shape
+    freqs, orders = [], []
+    for i in range(num_orders):
+        for j in range(num_modes):
+            freq = fn_poles[i, j]
+            if not np.isnan(freq):
+                model_order = num_orders - i
+                if freqlim is None or (freqlim[0] <= freq <= freqlim[1]):
+                    freqs.append(freq)
+                    orders.append(model_order)
+
+    ax.scatter(freqs, orders, color='red', s=10)
+
+    ax.set_title("Stabilization Diagram")
+    ax.set_xlabel("Frequency (Hz)")
+    ax.set_ylabel("Model order")
+    ax.set_ylim(0, num_orders + 1)
+    ax.yaxis.set_major_locator(ticker.MaxNLocator(integer=True))
+    ax.grid(True)
+
+    if freqlim:
+        ax.set_xlim(freqlim)
+    else:
+        ax.set_xlim(0, max(freqs) * 1.05)
+
+    fig.tight_layout()
+    fig.canvas.draw()
+    fig.canvas.flush_events()
+
+    return fig, ax

src/functions/util.py

@@ -0,0 +1,33 @@
+from typing import Any
+import numpy as np
+
+# pylint: disable=R0911
+def convert_numpy_to_list(obj: Any) -> Any:
+    """
+    Recursively convert NumPy arrays and complex numbers to JSON-safe types.
+
+    Args:
+        obj: Any data type.
+
+    Returns:
+        A fully JSON-serializable version of the input.
+    """
+    if isinstance(obj, dict):
+        return {k: convert_numpy_to_list(v) for k, v in obj.items()}
+    if isinstance(obj, list):
+        return [convert_numpy_to_list(item) for item in obj]
+    if isinstance(obj, tuple):
+        return tuple(convert_numpy_to_list(item) for item in obj)
+    if isinstance(obj, np.ndarray):
+        return convert_numpy_to_list(obj.tolist())
+    if isinstance(obj, complex):
+        return {"real": obj.real, "imag": obj.imag}
+    if isinstance(obj, (np.integer, np.floating)):
+        return obj.item()
+    try:
+        # fallback: try converting if it's a NumPy scalar or unknown object
+        if hasattr(obj, 'item'):
+            return obj.item()
+    except Exception:
+        pass
+    return obj

src/methods/constants.py

@@ -0,0 +1,7 @@
+DEFAULT_FS = 250 # In case the Fs from metadata doesn't arrive
+
+MIN_SAMPLES_NEEDED = 540  # Minimum samples for running sysid
+
+BLOCK_SHIFT = 30 # Used in sysID algorithm
+
+MODEL_ORDER = 20 # Used in sysID algorithm