yostlabs 2025.9.18__tar.gz → 2025.10.24__tar.gz

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: yostlabs
-Version: 2025.9.18
+Version: 2025.10.24
 Summary: Python resources and API for 3Space sensors from Yost Labs Inc.
 Project-URL: Homepage, https://yostlabs.com/
 Project-URL: Repository, https://github.com/YostLabs/3SpacePythonPackage/tree/main

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/pyproject.toml

@@ -5,7 +5,7 @@ build-backend = "hatchling.build"
 [project]
 name = "yostlabs"
 #If uploading again on the same day, add a fourth number
-version = "2025.09.18"
+version = "2025.10.24"
 authors = [
   { name="Yost Labs Inc.", email="techsupport@yostlabs.com" },
   { name="Andy Riedlinger", email="techsupport@yostlabs.com" },

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/src/yostlabs/communication/ble.py

@@ -7,6 +7,7 @@ from bleak.backends.device import BLEDevice
 from bleak.backends.scanner import AdvertisementData
 from bleak.backends.characteristic import BleakGATTCharacteristic
 from bleak.exc import BleakDeviceNotFoundError
+from dataclasses import dataclass
 
 #Services
 NORDIC_UART_SERVICE_UUID = "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
@@ -23,6 +24,12 @@ HARDWARE_REVISION_STRING_UUID = "00002a27-0000-1000-8000-00805f9b34fb"
 SERIAL_NUMBER_STRING_UUID = "00002a25-0000-1000-8000-00805f9b34fb"
 MANUFACTURER_NAME_STRING_UUID = "00002a29-0000-1000-8000-00805f9b34fb"
 
+@dataclass
+class ThreespaceBLENordicUartProfile:
+    SERVICE_UUID: str
+    RX_UUID: str
+    TX_UUID: str
+
 class TssBLENoConnectionError(Exception): ...
 
 def ylBleEventLoopThread(loop: asyncio.AbstractEventLoop):
@@ -36,6 +43,9 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     EVENT_LOOP = None
     EVENT_LOOP_THREAD = None
 
+    DEFAULT_PROFILE = ThreespaceBLENordicUartProfile(NORDIC_UART_SERVICE_UUID, NORDIC_UART_RX_UUID, NORDIC_UART_TX_UUID)
+    REGISTERED_PROFILES: list[ThreespaceBLENordicUartProfile] = [DEFAULT_PROFILE]
+
     @classmethod
     def __lazy_event_loop_init(cls):
         if cls.EVENT_LOOP is None:
@@ -43,7 +53,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
             cls.EVENT_LOOP_THREAD = threading.Thread(target=ylBleEventLoopThread, args=(cls.EVENT_LOOP,), daemon=True)
             cls.EVENT_LOOP_THREAD.start()
 
-    def __init__(self, ble: BleakClient | BLEDevice | str, discover_name: bool = True, discovery_timeout=5, error_on_disconnect=True):
+    def __init__(self, ble: BleakClient | BLEDevice | str, discover_name: bool = True, discovery_timeout=5, error_on_disconnect=True, adv: AdvertisementData = None):
         """
         Parameters
         ----------
@@ -54,6 +64,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         if it is expected that the sensor will frequently go in and out of range and the user wishes to preserve data (such as streaming)
         """
         self.__lazy_event_loop_init()
+        self.adv = adv
         bleak_options = { "timeout": discovery_timeout, "disconnected_callback": self.__on_disconnect }
         if isinstance(ble, BleakClient):    #Actual client
             self.client = ble
@@ -76,6 +87,19 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         else:
             raise TypeError("Invalid type for creating a ThreespaceBLEComClass:", type(ble), ble)
 
+        #Select the profile
+        self.profile = None
+        if self.adv is not None and len(self.adv.service_uuids) > 0:
+            for service_uuid in self.adv.service_uuids:
+                self.profile = self.get_profile(service_uuid)
+                if self.profile is not None:
+                    break
+            if self.profile is None:
+                self.profile = ThreespaceBLEComClass.DEFAULT_PROFILE
+                raise Exception(f"Unknown Service UUIDS: {self.adv.service_uuids}")
+        else:
+            self.profile = ThreespaceBLEComClass.DEFAULT_PROFILE
+
         self.__timeout = self.DEFAULT_TIMEOUT
 
         self.buffer = bytearray()
@@ -101,7 +125,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     async def __async_open(self):
         self.data_read_event = asyncio.Event()
         await self.client.connect()
-        await self.client.start_notify(NORDIC_UART_TX_UUID, self.__on_data_received)
+        await self.client.start_notify(self.profile.TX_UUID, self.__on_data_received)
 
     def open(self):
         #If trying to open while already open, this infinitely loops
@@ -151,7 +175,7 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         while start_index < len(bytes):
             end_index = min(len(bytes), start_index + self.max_packet_size) #Can only send max_packet_size data per call to write_gatt_char
             asyncio.run_coroutine_threadsafe(
-                self.client.write_gatt_char(NORDIC_UART_RX_UUID, bytes[start_index:end_index], response=False),
+                self.client.write_gatt_char(self.profile.RX_UUID, bytes[start_index:end_index], response=False),
                 self.EVENT_LOOP).result()
             start_index = end_index
     
@@ -244,8 +268,16 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     def address(self) -> str:
         return self.client.address    
 
-    SCANNER = None
+    @classmethod
+    def get_profile(cls, service_uuid: str):
+        for profile in cls.REGISTERED_PROFILES:
+            if profile.SERVICE_UUID == service_uuid:
+                return profile
+        return None
+
+    SCANNER: BleakScanner = None
     SCANNER_LOCK = None
+    SCANNER_RUNNING = False
 
     SCANNER_CONTINOUS = False   #Controls if scanning will continously run
     SCANNER_TIMEOUT = 5         #Controls the scanners timeout
@@ -255,23 +287,94 @@ class ThreespaceBLEComClass(ThreespaceComClass):
     #Format: Address - dict = { device: ..., adv: ..., last_found: ... }
     discovered_devices: dict[str,dict] = {}
 
+    @classmethod
+    def set_profiles(cls, profiles: list[ThreespaceBLENordicUartProfile]):
+        cls.REGISTERED_PROFILES = profiles
+        if cls.SCANNER is not None:
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
+            cls.__remove_unused_profiles()
+
+    @classmethod
+    def register_profile(cls, profile: ThreespaceBLENordicUartProfile):        
+        if any(v.SERVICE_UUID == profile.SERVICE_UUID for v in cls.REGISTERED_PROFILES): return
+        cls.REGISTERED_PROFILES.append(profile)
+        if cls.SCANNER is not None:
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
+    
+    @classmethod
+    def unregister_profile(cls, service_uuid: str|ThreespaceBLENordicUartProfile):
+        if isinstance(service_uuid, ThreespaceBLENordicUartProfile):
+            service_uuid = service_uuid.SERVICE_UUID
+        index = None
+        for i in range(len(cls.REGISTERED_PROFILES)):
+            if cls.REGISTERED_PROFILES[i].SERVICE_UUID == service_uuid:
+                index = i
+                break
+        del cls.REGISTERED_PROFILES[index]
+        if cls.SCANNER is not None:
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
+            cls.__remove_unused_profiles()
+
+    @classmethod
+    def __remove_unused_profiles(cls):
+        if cls.SCANNER is None: return
+        to_remove = []
+        valid_service_uuids = [v.SERVICE_UUID for v in cls.REGISTERED_PROFILES]
+        with cls.SCANNER_LOCK:
+            for address in cls.discovered_devices:
+                adv: AdvertisementData = cls.discovered_devices[address]["adv"]
+                if not any(uuid in valid_service_uuids for uuid in adv.service_uuids):
+                    to_remove.append(address)
+            for address in to_remove:
+                del cls.discovered_devices[address]
+
+    #Scanner should be created inside of the Async Context that will use it
+    @classmethod
+    async def create_scanner(cls):
+        uuids = [v.SERVICE_UUID for v in cls.REGISTERED_PROFILES]
+        restart_scanner = cls.SCANNER_RUNNING
+        with cls.SCANNER_LOCK:
+            if restart_scanner:
+                await cls.__async_stop_scanner()
+            cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=uuids)
+            if restart_scanner:
+                await cls.__async_start_scanner()
+
     @classmethod
     def __lazy_init_scanner(cls):
         cls.__lazy_event_loop_init()
         if cls.SCANNER is None:
             cls.SCANNER_LOCK = threading.Lock()
-            #Scanner should be created inside of the Async Context that will use it
-            async def create_scanner():
-                cls.SCANNER = BleakScanner(detection_callback=cls.__detection_callback, service_uuids=[NORDIC_UART_SERVICE_UUID])
-            asyncio.run_coroutine_threadsafe(create_scanner(), cls.EVENT_LOOP).result()
-
-            
+            asyncio.run_coroutine_threadsafe(cls.create_scanner(), cls.EVENT_LOOP).result()
 
     @classmethod
     def __detection_callback(cls, device: BLEDevice, adv: AdvertisementData):
         with cls.SCANNER_LOCK:
             cls.discovered_devices[device.address] = {"device": device, "adv": adv, "last_found": time.time()}
     
+    @classmethod
+    async def __async_start_scanner(cls):
+        if cls.SCANNER_RUNNING: return
+        await cls.SCANNER.start()
+        cls.SCANNER_RUNNING = True
+
+    @classmethod
+    async def __async_stop_scanner(cls):
+        if not cls.SCANNER_RUNNING: return
+        await cls.SCANNER.stop()
+        cls.SCANNER_RUNNING = False        
+
+    @classmethod
+    def __start_scanner(cls):
+        if cls.SCANNER_RUNNING: return
+        asyncio.run_coroutine_threadsafe(cls.__async_start_scanner(), cls.EVENT_LOOP).result()
+    
+    @classmethod
+    def __stop_scanner(cls):
+        if not cls.SCANNER_RUNNING: return
+        asyncio.run_coroutine_threadsafe(cls.__async_stop_scanner(), cls.EVENT_LOOP).result()
+        cls.__stop_scanner()
+
     @classmethod
     def set_scanner_continous(cls, continous: bool):
         """
@@ -285,9 +388,9 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         cls.__lazy_init_scanner()
         cls.SCANNER_CONTINOUS = continous
         if continous: 
-            asyncio.run_coroutine_threadsafe(cls.SCANNER.start(), cls.EVENT_LOOP).result()
+            cls.__start_scanner()
         else: 
-            asyncio.run_coroutine_threadsafe(cls.SCANNER.stop(), cls.EVENT_LOOP).result()
+            cls.__stop_scanner()
 
     @classmethod
     def update_nearby_devices(cls):
@@ -338,4 +441,4 @@ class ThreespaceBLEComClass(ThreespaceComClass):
         cls.update_nearby_devices()
         with cls.SCANNER_LOCK:
             for device_info in cls.discovered_devices.values():
-                yield(ThreespaceBLEComClass(device_info["device"]))
+                yield(ThreespaceBLEComClass(device_info["device"], adv=device_info["adv"]))

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/src/yostlabs/communication/serial.py

@@ -1,16 +1,25 @@
 from yostlabs.communication.base import *
 import serial
 import serial.tools.list_ports
+from serial.tools.list_ports_common import ListPortInfo
 import time
 
-
 class ThreespaceSerialComClass(ThreespaceComClass):
-
     PID_V3_MASK = 0x3000
-    PID_BOOTLOADER = 0x1000
 
     VID = 0x2476
 
+    PID_BOOTLOADER = 0x1000
+    PID_EMBED = 0x3040
+    PID_DL = 0x3050
+
+    PID_TO_STR_DICT = {
+        PID_EMBED: "EM",
+        PID_DL: "DL",
+        PID_BOOTLOADER: "BOOT"
+    }
+
+
     DEFAULT_BAUDRATE = 115200
     DEFAULT_TIMEOUT = 2
 
@@ -18,12 +27,11 @@ class ThreespaceSerialComClass(ThreespaceComClass):
         if isinstance(ser, serial.Serial):
             self.ser = ser
         elif isinstance(ser, str):
-            self.ser = serial.Serial(ser, baudrate=ThreespaceSerialComClass.DEFAULT_BAUDRATE, timeout=ThreespaceSerialComClass.DEFAULT_TIMEOUT)
+            self.ser = serial.Serial(None, baudrate=ThreespaceSerialComClass.DEFAULT_BAUDRATE, timeout=ThreespaceSerialComClass.DEFAULT_TIMEOUT)
+            self.ser.port = ser
         else:
             raise TypeError("Invalid type for creating a ThreespaceSerialComClass:", type(ser), ser)
 
-        self.ser = ser
-
         self.peek_buffer = bytearray()
         self.peek_length = 0
 
@@ -120,6 +128,22 @@ class ThreespaceSerialComClass(ThreespaceComClass):
     @property
     def name(self) -> str:
         return self.ser.port
+    
+    @property
+    def suffix(self) -> str:
+        return self.pid_to_str(self.get_port_info().pid) 
+    
+    def get_port_info(self):
+        ports = serial.tools.list_ports.comports()
+        for port in ports:
+            if port.device == self.ser.port:
+                return port
+        return None
+
+    @staticmethod
+    def is_threespace_port(port: ListPortInfo):
+        cls = ThreespaceSerialComClass
+        return port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER)
 
     #This is not part of the ThreespaceComClass interface, but is useful as a utility for those directly using the ThreespaceSerialComClass
     @staticmethod 
@@ -127,7 +151,7 @@ class ThreespaceSerialComClass(ThreespaceComClass):
         cls = ThreespaceSerialComClass
         ports = serial.tools.list_ports.comports()
         for port in ports:
-            if port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER): 
+            if cls.is_threespace_port(port):
                 yield port
 
     @staticmethod
@@ -139,7 +163,12 @@ class ThreespaceSerialComClass(ThreespaceComClass):
         cls = ThreespaceSerialComClass
         ports = serial.tools.list_ports.comports()
         for port in ports:
-            if port.vid == cls.VID and (port.pid & cls.PID_V3_MASK == cls.PID_V3_MASK or port.pid == cls.PID_BOOTLOADER): 
+            if cls.is_threespace_port(port):
                 ser = serial.Serial(None, baudrate=default_baudrate, timeout=default_timeout) #By setting port as None, can create an object without immediately opening the port
                 ser.port = port.device #Now assign the port, allowing the serial object to exist without being opened yet
-                yield ThreespaceSerialComClass(ser)
+                yield ThreespaceSerialComClass(ser)
+
+    @classmethod
+    def pid_to_str(cls, pid):
+        if pid not in cls.PID_TO_STR_DICT: return "Unknown"
+        return cls.PID_TO_STR_DICT[pid]

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/src/yostlabs/math/quaternion.py

@@ -51,6 +51,15 @@ def quat_from_axis_angle(axis: list[float], angle: float):
     quat.append(math.cos(angle / 2))
     return quat
 
+#There are multiple valid quats that can be returned by this. The intention of this function
+#is to be able to rotate an arrow by the quat such that it points the correct direction. The rotation
+#of that arrow along its axis may differ though
+def quat_from_one_vector(vec: list[float]):
+    vec = _vec.vec_normalize(vec)
+    perpendicular = _vec.vec_normalize(_vec.vec_cross([0, 0, 1], vec))
+    angle = math.acos(_vec.vec_dot([0, 0, 1], vec))
+    return quat_from_axis_angle(perpendicular, angle)
+
 def quat_from_two_vectors(forward: list[float], down: list[float]):
     """
     This function requires two orthogonal vectors to work

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/src/yostlabs/tss3/api.py

@@ -421,6 +421,8 @@ class StreamableCommands(Enum):
     GetGpsHdop = 218
     GetGpsSattelites = 219
 
+    GetLedColor = 238
+
     GetButtonState = 250
 
 THREESPACE_AWAIT_COMMAND_FOUND = 0
@@ -1674,7 +1676,8 @@ class ThreespaceSensor:
     def getStreamingLabel(self, cmd_num: int) -> ThreespaceCmdResult[str]: ...
     def setCursor(self, cursor_index: int) -> ThreespaceCmdResult[None]: ...
     def getLastLogCursorInfo(self) -> ThreespaceCmdResult[tuple[int,str]]: ...
-    def pauseLogStreaming(self, pause: bool) -> ThreespaceCmdResult[None]: ...           
+    def pauseLogStreaming(self, pause: bool) -> ThreespaceCmdResult[None]: ...  
+    def getLedColor(self) -> ThreespaceCmdResult[list[float]]: ...           
 
 THREESPACE_GET_STREAMING_BATCH_COMMAND_NUM = 84
 THREESPACE_START_STREAMING_COMMAND_NUM = 85
@@ -1817,6 +1820,8 @@ _threespace_commands: list[ThreespaceCommand] = [
     ThreespaceCommand("softwareReset", THREESPACE_SOFTWARE_RESET_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__softwareReset),
     ThreespaceCommand("enterBootloader", THREESPACE_ENTER_BOOTLOADER_COMMAND_NUM, "", "", custom_func=ThreespaceSensor._ThreespaceSensor__enterBootloader),
 
+    ThreespaceCommand("getLedColor", 238, "", "fff"),
+
     ThreespaceCommand("getButtonState", 250, "", "b"),
 ]
 

{yostlabs-2025.9.18 → yostlabs-2025.10.24}/src/yostlabs/tss3/consts.py

@@ -61,4 +61,6 @@ THREESPACE_SN_FAMILY_TO_NAME = {
     0x15 : THREESPACE_FAMILY_BLUETOOTH,
     0x16 : THREESPACE_FAMILY_DATA_LOGGER,
     0x17 : THREESPACE_FAMILY_MICRO_USB
-}
+}
+
+THREESPACE_ERR_COMMIT_FS_LOCKED=8

yostlabs-2025.10.24/src/yostlabs/tss3/utils/calibration.py

@@ -0,0 +1,363 @@
+import yostlabs.math.quaternion as quat
+import yostlabs.math.vector as vec
+
+import numpy as np
+from dataclasses import dataclass
+import copy
+
+class ThreespaceGradientDescentCalibration:
+
+    @dataclass
+    class StageInfo:
+        start_vector: int
+        end_vector: int
+        stage: int
+        scale: float
+
+        count: int = 0
+
+    MAX_SCALE = 1000000000
+    MIN_SCALE = 1
+    STAGES = [
+        StageInfo(0, 6, 0, MAX_SCALE),
+        StageInfo(0, 12, 1, MAX_SCALE),
+        StageInfo(0, 24, 2, MAX_SCALE)
+    ]
+
+    #Note that each entry has a positive and negative vector included in this list
+    CHANGE_VECTORS = [
+        np.array([0,0,0,0,0,0,0,0,0,.0001,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,-.0001,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,.0001,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,-.0001,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,0,.0001], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,0,0,0,-.0001], dtype=np.float64), #First 6 only try to change the bias
+        np.array([.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([-.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,.001,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,-.001,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,.001,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,0,-.001,0,0,0], dtype=np.float64), #Next 6 only try to change the scale
+        np.array([0,.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,-.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,-.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,-.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,.0001,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,-.0001,0,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,.0001,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,-.0001,0,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,.0001,0,0,0,0], dtype=np.float64),
+        np.array([0,0,0,0,0,0,0,-.0001,0,0,0,0], dtype=np.float64), #Next 12 only try to change the shear
+    ]
+
+    def __init__(self, relative_sensor_orients: list[np.ndarray[float]], no_inverse=False):
+        """
+        Params
+        ------
+        relative_sensor_orients : The orientation of the sensor during which each sample is taken if it was tared as if pointing into the screen. 
+        The inverse of these will be used to calculate where the axes should be located relative to the sensor
+        no_inverse : The relative_sensor_orients will be treated as the sample_rotations
+        """
+        if no_inverse:
+            self.rotation_quats = relative_sensor_orients
+        else:
+            self.rotation_quats = [np.array(quat.quat_inverse(orient)) for orient in relative_sensor_orients]
+
+    def apply_parameters(self, sample: np.ndarray[float], params: np.ndarray[float]):
+        bias = params[9:]
+        scale = params[:9]
+        scale = scale.reshape((3, 3))
+        return scale @ (sample + bias)
+
+    def rate_parameters(self, params: np.ndarray[float], samples: list[np.ndarray[float]], targets: list[np.ndarray[float]]):
+        total_error = 0
+        for i in range(len(samples)):
+            sample = samples[i]
+            target = targets[i]
+
+            sample = self.apply_parameters(sample, params)
+            
+            error = target - sample
+            total_error += vec.vec_len(error)
+        return total_error
+
+    def generate_target_list(self, origin: np.ndarray):
+        targets = []
+        for orient in self.rotation_quats:
+            new_vec = np.array(quat.quat_rotate_vec(orient, origin), dtype=np.float64)
+            targets.append(new_vec)
+        return targets
+
+    def __get_stage(self, stage_number: int):
+        if stage_number >= len(self.STAGES):
+            return None
+        #Always get a shallow copy of the stage so can modify without removing the initial values
+        return copy.copy(self.STAGES[stage_number])
+
+    def calculate(self, samples: list[np.ndarray[float]], origin: np.ndarray[float], verbose=False, max_cycles_per_stage=1000):
+        targets = self.generate_target_list(origin)
+        initial_params = np.array([1,0,0,0,1,0,0,0,1,0,0,0], dtype=np.float64)
+        stage = self.__get_stage(0)
+
+        best_params = initial_params
+        best_rating = self.rate_parameters(best_params, samples, targets)
+        count = 0
+        while True:
+            last_best_rating = best_rating
+            params = best_params
+
+            #Apply all the changes to see if any improve the result
+            for change_index in range(stage.start_vector, stage.end_vector):
+                change_vector = self.CHANGE_VECTORS[change_index]
+                new_params = params + (change_vector * stage.scale)
+                rating = self.rate_parameters(new_params, samples, targets)
+
+                #A better rating, store it
+                if rating < best_rating:
+                    best_params = new_params
+                    best_rating = rating
+            
+            if verbose and count % 100 == 0:
+                print(f"Round {count}: {best_rating=} {stage=}")
+            
+            #Decide if need to go to the next stage or not
+            count += 1
+            stage.count += 1
+            if stage.count >= max_cycles_per_stage:
+                stage = self.__get_stage(stage.stage + 1)
+                if stage is None:
+                    if verbose: print("Done from reaching count limit")
+                    break
+                if verbose: print("Going to next stage from count limit")
+                
+            if best_rating == last_best_rating: #The rating did not improve
+                if stage.scale == self.MIN_SCALE: #Go to the next stage since can't get any better in this stage!
+                    stage = self.__get_stage(stage.stage + 1)
+                    if stage is None:
+                        if verbose: print("Done from exhaustion")
+                        break
+                    if verbose: print("Going to next stage from exhaustion")
+                else:   #Reduce the size of the changes to hopefully get more accurate tuning
+                    stage.scale *= 0.1  
+                    if stage.scale < self.MIN_SCALE:
+                        stage.scale = self.MIN_SCALE
+            else: #Rating got better! To help avoid falling in a local minimum, increase the size of the change to see if that could make it better
+                stage.scale *= 1.1
+        
+        if verbose:
+            print(f"Final Rating: {best_rating}")
+            print(f"Final Params: {best_params}")
+
+        return best_params
+    
+def fibonacci_sphere(samples=1000):
+    points = []
+    phi = np.pi * (3. - np.sqrt(5.))  # golden angle
+
+    for i in range(samples):
+        y = 1 - (i / float(samples - 1)) * 2  # y goes from 1 to -1
+        radius = np.sqrt(1 - y * y)
+        theta = phi * i
+        x = np.cos(theta) * radius
+        z = np.sin(theta) * radius
+        points.append((x, y, z))
+
+    return np.array(points)
+
+class ThreespaceSphereCalibration:
+
+    def __init__(self, max_comparison_points=500):
+        self.buffer = 0.1
+        self.test_points = fibonacci_sphere(samples=max_comparison_points)
+        self.clear()
+
+    def process_point(self, raw_mag: list[float]):
+        if len(self.samples) == 0:
+            self.samples = np.array([raw_mag])
+            return True
+        
+        raw_mag = np.array(raw_mag, dtype=np.float64)
+        new_len = np.linalg.norm(raw_mag)
+
+        avg_len = np.linalg.norm(self.samples, axis=1)
+        avg_len = (avg_len + new_len) / 2 * self.buffer
+
+        dist = np.linalg.norm(self.samples - raw_mag, axis=1)
+        if np.any(dist < avg_len):
+            return False
+        
+        self.samples = np.concatenate((self.samples, [raw_mag]))
+        self.__update_density(raw_mag / new_len)
+        return True
+    
+    def __update_density(self, normalized_point: np.ndarray):
+        #First check to see if the new point is the closest point for any of the previous points
+        dots = np.sum(self.test_points * normalized_point, axis=1)
+        self.closest_dot = np.maximum(self.closest_dot, dots)
+        self.largest_delta_index = np.argmin(self.closest_dot)
+        self.largest_delta = np.rad2deg(np.acos(self.closest_dot[self.largest_delta_index]))
+
+    def clear(self):
+        self.samples = np.array([])
+        self.closest_dot = np.array([-1] * len(self.test_points))
+        self.largest_delta = 180
+        self.largest_delta_index = 0
+
+    @property
+    def sparsest_vector(self):
+        return self.test_points[self.largest_delta_index]
+
+    def calculate(self):
+        """
+        Returns matrix and bias
+        """
+        comp_A, comp_b, comp_d = ThreespaceSphereCalibration.alternating_least_squares(np.array(self.samples))
+        comp_A, comp_b = ThreespaceSphereCalibration.make_calibration_params(comp_A, comp_b, comp_d, 1)
+
+        comp_scale: float = comp_A[0][0] + comp_A[1][1] + comp_A[2][2]
+        comp_scale /= 3
+
+        comp_A /= comp_scale
+
+        return comp_A.flatten().tolist(), comp_b.tolist()
+
+    @staticmethod
+    def alternating_least_squares(data: np.ndarray[np.ndarray]) -> tuple[np.ndarray, np.ndarray, float]:
+        n = 3
+        m = len(data)
+
+        #Step 1
+        noise_variance = 0.1
+        sigma2 = noise_variance ** 2
+        rotini = data.T
+        rotini2 = rotini ** 2
+
+        T = np.zeros((5,n,m))
+        T[0,:,:] = 1
+        T[1,:,:] = rotini
+        T[2,:,:] = rotini2 - sigma2
+        T[3,:,:] = rotini2 * rotini - 3 * rotini * sigma2
+        T[4,:,:] = rotini2 * rotini2 - 6 * rotini2 * sigma2 + 3 * sigma2 * sigma2
+
+        #Step 2
+        one = np.ones((n+1,))
+        i = np.arange(1, n+1)
+        i = np.append(i, 0)
+
+        m1 = np.outer(i.T, one)
+        m2 = np.outer(one.T, i)
+
+        M = np.array([ThreespaceSphereCalibration.vec_s(m1), ThreespaceSphereCalibration.vec_s(m2)])
+
+        #Step 3
+        nb = int((n + 1) * n / 2 + n + 1)
+        R = np.zeros((nb,nb,n), dtype=np.int32)
+        
+        for p in range(nb):
+            for q in range(p, nb):
+                for i in range(1, n+1):
+                    R[p,q,i-1] = int(M[0,p] == i) + int(M[1,p] == i) + int(M[0,q] == i) + int(M[1,q] == i)
+
+        #Step 4
+        nals = np.zeros((nb, nb))
+        for p in range(nb):
+            for q in range(p, nb):
+                sum = 0
+                for l in range(m):
+                    prod = 1
+                    for i in range(n):
+                        prod *= T[R[p,q,i],i,l]
+                    sum += prod
+                nals[p,q] = sum
+
+        #Step 5
+        D2 = [i * (i + 1) / 2 for i in range(1, n+1)]
+        D = [d for d in range(1, (n+1) * n // 2 + 1) if not d in D2]
+
+        #Step 6
+        menorah_als = np.zeros((nb, nb))
+        
+        for p in range(nb):
+            for q in range(p, nb):
+                coeff = 2
+                if p + 1 in D and q + 1 in D:
+                    coeff = 4
+                elif not p + 1 in D and not q + 1 in D:
+                    coeff = 1
+                menorah_als[p,q] = coeff * nals[p,q]
+        
+        # Fill the lower triangle with the upper triangle values
+        i_lower, j_lower = np.tril_indices(nb, k=-1) 
+        menorah_als[i_lower, j_lower] = menorah_als[j_lower, i_lower]
+
+        #Step 7
+        eigenmat = menorah_als
+
+        #It is unclear if this is correct, there are differences in sign and positions
+        eigenvalues, eigenvectors = np.linalg.eig(eigenmat)
+        eigenvectors = eigenvectors.T
+
+        #Looks like this section gets the eigen vector with the largest eigen value?
+        combinedmatr = [[abs(eigenvalues[i]), eigenvectors[i]] for i in range(len(eigenvalues))]
+        combinedmatr.sort(key=lambda a: a[0], reverse=True)
+
+        bals = combinedmatr[-1][1]
+
+        #Step 8 : ensure normalized
+        bals = bals / np.linalg.norm(bals)
+
+        #Step 9:
+        triangle = n*(n+1)//2
+        A = ThreespaceSphereCalibration.inv_vec_s(bals[:triangle])
+        b = bals[triangle:nb-1]
+        d = bals[-1]
+
+        return A, b, d
+
+    @staticmethod
+    def make_calibration_params(Q: np.ndarray, u: np.ndarray, k: float, H_m: float) -> tuple[np.ndarray,np.ndarray]:
+        pa = np.linalg.inv(Q)
+        pb = u.T
+        b = np.dot(pa, pb) * 0.5
+
+        eigenvalues, V = np.linalg.eig(Q)
+        D = np.diag(eigenvalues)
+
+        vu_prod = np.dot(V.T, u.T)
+        p1a = np.dot(vu_prod.T, np.linalg.inv(D))
+        p1b = np.dot(p1a, vu_prod)
+        p1 = p1b - (4 * k)
+
+        alpha = 4 * (H_m ** 2) / p1
+
+        aD = np.diag(abs(alpha * eigenvalues) ** 0.5)
+
+        A = np.dot(np.dot(V, aD), V.T)
+        
+        return A, b
+
+    @staticmethod
+    def vec_s(matrix: np.ndarray):
+        rows, cols = np.tril_indices(matrix.shape[0])
+        return matrix[rows, cols]
+
+    @staticmethod
+    def inv_vec_s(vec: np.ndarray):
+        #Its unclear if this function works as intended. But this is how the suite does it.
+        size = int((-1 + (1 + 8 * len(vec)) ** 0.5) / 2)
+        matr = np.zeros((size,size))
+        base = 0
+        for i in range(size):
+            for j in range(i):
+                matr[i,j] = vec[base + j]
+                matr[j,i] = vec[base + j]
+            matr[i,i] = vec[base + i]
+            base += i + 1
+        
+        return matr
+
+    @property
+    def num_points(self):
+        return len(self.samples)

yostlabs-2025.9.18/src/yostlabs/tss3/utils/calibration.py

@@ -1,153 +0,0 @@
-import yostlabs.math.quaternion as quat
-import yostlabs.math.vector as vec
-
-import numpy as np
-from dataclasses import dataclass
-import copy
-
-class ThreespaceGradientDescentCalibration:
-
-    @dataclass
-    class StageInfo:
-        start_vector: int
-        end_vector: int
-        stage: int
-        scale: float
-
-        count: int = 0
-
-    MAX_SCALE = 1000000000
-    MIN_SCALE = 1
-    STAGES = [
-        StageInfo(0, 6, 0, MAX_SCALE),
-        StageInfo(0, 12, 1, MAX_SCALE),
-        StageInfo(0, 24, 2, MAX_SCALE)
-    ]
-
-    #Note that each entry has a positive and negative vector included in this list
-    CHANGE_VECTORS = [
-        np.array([0,0,0,0,0,0,0,0,0,.0001,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,0,-.0001,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,0,0,.0001,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,0,0,-.0001,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,0,0,0,.0001], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,0,0,0,-.0001], dtype=np.float64), #First 6 only try to change the bias
-        np.array([.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([-.001,0,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,.001,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,-.001,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,.001,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,0,-.001,0,0,0], dtype=np.float64), #Next 6 only try to change the scale
-        np.array([0,.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,-.0001,0,0,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,-.0001,0,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,-.0001,0,0,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,.0001,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,-.0001,0,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,.0001,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,-.0001,0,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,.0001,0,0,0,0], dtype=np.float64),
-        np.array([0,0,0,0,0,0,0,-.0001,0,0,0,0], dtype=np.float64), #Next 12 only try to change the shear
-    ]
-
-    def __init__(self, relative_sensor_orients: list[np.ndarray[float]], no_inverse=False):
-        """
-        Params
-        ------
-        relative_sensor_orients : The orientation of the sensor during which each sample is taken if it was tared as if pointing into the screen. 
-        The inverse of these will be used to calculate where the axes should be located relative to the sensor
-        no_inverse : The relative_sensor_orients will be treated as the sample_rotations
-        """
-        if no_inverse:
-            self.rotation_quats = relative_sensor_orients
-        else:
-            self.rotation_quats = [np.array(quat.quat_inverse(orient)) for orient in relative_sensor_orients]
-
-    def apply_parameters(self, sample: np.ndarray[float], params: np.ndarray[float]):
-        bias = params[9:]
-        scale = params[:9]
-        scale = scale.reshape((3, 3))
-        return scale @ (sample + bias)
-
-    def rate_parameters(self, params: np.ndarray[float], samples: list[np.ndarray[float]], targets: list[np.ndarray[float]]):
-        total_error = 0
-        for i in range(len(samples)):
-            sample = samples[i]
-            target = targets[i]
-
-            sample = self.apply_parameters(sample, params)
-            
-            error = target - sample
-            total_error += vec.vec_len(error)
-        return total_error
-
-    def generate_target_list(self, origin: np.ndarray):
-        targets = []
-        for orient in self.rotation_quats:
-            new_vec = np.array(quat.quat_rotate_vec(orient, origin), dtype=np.float64)
-            targets.append(new_vec)
-        return targets
-
-    def __get_stage(self, stage_number: int):
-        if stage_number >= len(self.STAGES):
-            return None
-        #Always get a shallow copy of the stage so can modify without removing the initial values
-        return copy.copy(self.STAGES[stage_number])
-
-    def calculate(self, samples: list[np.ndarray[float]], origin: np.ndarray[float], verbose=False, max_cycles_per_stage=1000):
-        targets = self.generate_target_list(origin)
-        initial_params = np.array([1,0,0,0,1,0,0,0,1,0,0,0], dtype=np.float64)
-        stage = self.__get_stage(0)
-
-        best_params = initial_params
-        best_rating = self.rate_parameters(best_params, samples, targets)
-        count = 0
-        while True:
-            last_best_rating = best_rating
-            params = best_params
-
-            #Apply all the changes to see if any improve the result
-            for change_index in range(stage.start_vector, stage.end_vector):
-                change_vector = self.CHANGE_VECTORS[change_index]
-                new_params = params + (change_vector * stage.scale)
-                rating = self.rate_parameters(new_params, samples, targets)
-
-                #A better rating, store it
-                if rating < best_rating:
-                    best_params = new_params
-                    best_rating = rating
-            
-            if verbose and count % 100 == 0:
-                print(f"Round {count}: {best_rating=} {stage=}")
-            
-            #Decide if need to go to the next stage or not
-            count += 1
-            stage.count += 1
-            if stage.count >= max_cycles_per_stage:
-                stage = self.__get_stage(stage.stage + 1)
-                if stage is None:
-                    if verbose: print("Done from reaching count limit")
-                    break
-                if verbose: print("Going to next stage from count limit")
-                
-            if best_rating == last_best_rating: #The rating did not improve
-                if stage.scale == self.MIN_SCALE: #Go to the next stage since can't get any better in this stage!
-                    stage = self.__get_stage(stage.stage + 1)
-                    if stage is None:
-                        if verbose: print("Done from exhaustion")
-                        break
-                    if verbose: print("Going to next stage from exhaustion")
-                else:   #Reduce the size of the changes to hopefully get more accurate tuning
-                    stage.scale *= 0.1  
-                    if stage.scale < self.MIN_SCALE:
-                        stage.scale = self.MIN_SCALE
-            else: #Rating got better! To help avoid falling in a local minimum, increase the size of the change to see if that could make it better
-                stage.scale *= 1.1
-        
-        if verbose:
-            print(f"Final Rating: {best_rating}")
-            print(f"Final Params: {best_params}")
-
-        return best_params