From 5eae012d0007c02006752a67b8ad8f457ab85149 Mon Sep 17 00:00:00 2001 From: Nicolas Kruse Date: Tue, 31 Mar 2026 11:34:46 +0200 Subject: [PATCH 1/3] Updated quaternion handling for usage with grad() function --- src/copapy/_autograd.py | 10 ++++++++-- src/copapy/_quaternion.py | 5 ++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/copapy/_autograd.py b/src/copapy/_autograd.py index 4840805..2413da2 100644 --- a/src/copapy/_autograd.py +++ b/src/copapy/_autograd.py @@ -1,3 +1,5 @@ +from copapy._quaternion import quaternion + from . import value, vector, tensor import copapy.backend as cpb from typing import Any, Sequence, overload @@ -12,8 +14,10 @@ def grad(x: Any, y: vector[Any]) -> vector[float]: ... @overload def grad(x: Any, y: tensor[Any]) -> tensor[float]: ... @overload +def grad(x: Any, y: quaternion) -> quaternion: ... +@overload def grad(x: Any, y: Sequence[value[Any]]) -> list[unifloat]: ... -def grad(x: Any, y: value[Any] | Sequence[value[Any]] | vector[Any] | tensor[Any]) -> Any: +def grad(x: Any, y: value[Any] | Sequence[value[Any]] | vector[Any] | tensor[Any] | quaternion) -> Any: """Returns the partial derivative dx/dy where x needs to be a scalar and y might be a scalar, a list of scalars, a vector or matrix. It uses automatic differentiation in reverse-mode. @@ -32,7 +36,7 @@ def grad(x: Any, y: value[Any] | Sequence[value[Any]] | vector[Any] | tensor[Any if isinstance(y, tensor): y_set = {v.get_scalar(0) for v in y.flatten()} else: - assert isinstance(y, Sequence) or isinstance(y, vector) + assert isinstance(y, Sequence) or isinstance(y, vector) or isinstance(y, quaternion) y_set = set(y) edges = cpb.get_all_dag_edges_between([x.net.source], (v.net.source for v in y_set if isinstance(v, value))) @@ -125,6 +129,8 @@ def grad(x: Any, y: value[Any] | Sequence[value[Any]] | vector[Any] | tensor[Any return grad_dict[y.net] if isinstance(y, vector): return vector(grad_dict[yi.net] if isinstance(yi, value) else 0.0 for yi in y.values) + if isinstance(y, quaternion): + return quaternion(grad_dict[yi.net] if isinstance(yi, value) else 0.0 for yi in y.values) if isinstance(y, tensor): return tensor([grad_dict[yi.net] if isinstance(yi, value) else 0.0 for yi in y.values], y.shape) return [grad_dict[yi.net] for yi in y] diff --git a/src/copapy/_quaternion.py b/src/copapy/_quaternion.py index 6750a70..b1445dc 100644 --- a/src/copapy/_quaternion.py +++ b/src/copapy/_quaternion.py @@ -1,4 +1,4 @@ -from typing import overload, Iterable, Callable, Any +from typing import overload, Iterable, Callable, Any, Iterator from ._vectors import vector from ._tensors import tensor import copapy as cp @@ -208,6 +208,9 @@ class quaternion(ArrayType[float]): """ return quaternion(func(x) for x in self.values) + def __iter__(self) -> Iterator[value[float] | float]: + return iter(self.values) + def __neg__(self) -> 'quaternion': return quaternion(-self.w, -self.x, -self.y, -self.z) From 5d3d6bdf63e880b2875d19a0911bb85506cdf862 Mon Sep 17 00:00:00 2001 From: Nicolas Kruse Date: Tue, 31 Mar 2026 11:35:15 +0200 Subject: [PATCH 2/3] sensor fusion quaternion test added --- tests/test_quaternion.py | 41 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 39 insertions(+), 2 deletions(-) diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 88f1134..b5de233 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -1,7 +1,7 @@ import math -from copapy import quaternion, tensor -from copapy import vector, Target +from copapy import quaternion, tensor, vector, Target import copapy as cp +import pytest def isclose(a, b, rel_tol=1e-9, abs_tol=0.0): @@ -294,3 +294,40 @@ def test_satellite_attitude_correction(): assert isclose(result_normal[0], expected_rotated[0], abs_tol=1e-6) assert isclose(result_normal[1], expected_rotated[1], abs_tol=1e-6) assert isclose(result_normal[2], expected_rotated[2], abs_tol=1e-6) + + +def test_sensor_fusion(): + # Based on Sebastian O. H. Madgwick's sensor fusion algorithm for orientation estimation. + # https://x-io.co.uk/open-source-imu-and-ahrs-algorithms + + def update_orientation(q: quaternion, gyro: vector[float], accel: vector[float], dt: float = 0.01): + # Compute the cost function and its gradient + objective = q.rotate_vector(vector([0.0, 0.0, 1.0])) - accel.normalize() + cost = 0.5 * objective.dot(objective) + gradient = cp.grad(cost, q).normalize() + + # Quaternion derivative from gyroscope measurements + gyro_quat = cp.quaternion(0.0, *gyro) + q_dot_gyro = 0.5 * (q @ gyro_quat) + + # Update quaternion using gradient descent + q_dot = q_dot_gyro - 0.1 * gradient + + return (q + q_dot * dt).normalize() + + q: quaternion = quaternion(cp.value(0.7071), cp.value(0.7071), cp.value(0.0), cp.value(0.0)) # Initial orientation (45 degrees around X-axis) + gyro = vector([0.01, 0.02, 0.015]) + accel = vector([0.0, 0.0, 1.0]) + + new_q = update_orientation(q, gyro, accel) + + tg = Target() + tg.compile(new_q) + tg.run() + + new_q_value = tg.read_value(new_q) + + assert pytest.approx(new_q_value[0], abs=1e-4) == 0.7072948217391968 + assert pytest.approx(new_q_value[1], abs=1e-4) == 0.7069186568260193 + assert pytest.approx(new_q_value[2], abs=1e-4) == 0.7660913727013394e-05 + assert pytest.approx(new_q_value[3], abs=1e-4) == 0.00012362639245111495 \ No newline at end of file From 139cfad9dd4535d94cc77fd22b180b3a9f54e5a1 Mon Sep 17 00:00:00 2001 From: Nicolas Date: Sun, 5 Apr 2026 13:54:47 +0200 Subject: [PATCH 3/3] __repr__ and type hints for quaternion class updated --- src/copapy/_autograd.py | 2 ++ src/copapy/_quaternion.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/copapy/_autograd.py b/src/copapy/_autograd.py index 2413da2..623879c 100644 --- a/src/copapy/_autograd.py +++ b/src/copapy/_autograd.py @@ -31,6 +31,8 @@ def grad(x: Any, y: value[Any] | Sequence[value[Any]] | vector[Any] | tensor[Any """ assert isinstance(x, value), f"Argument x for grad function must be a copapy value but is {type(x)}." + y_set: set[value[float] | float] + if isinstance(y, value): y_set = {y} if isinstance(y, tensor): diff --git a/src/copapy/_quaternion.py b/src/copapy/_quaternion.py index b1445dc..f9b3dfe 100644 --- a/src/copapy/_quaternion.py +++ b/src/copapy/_quaternion.py @@ -218,7 +218,7 @@ class quaternion(ArrayType[float]): return self.norm() def __repr__(self) -> str: - return f"vector({self.values})" + return f"quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})" def __len__(self) -> int: return len(self.values)