add test

Author: baggepinnen

Project.toml

@@ -1,20 +1,25 @@
 name = "DyadBotComponents"
 uuid = "6baa8624-4e17-4d27-96ac-5a32f577bc5c"
-authors = ["Fredrik Bagge Carlson"]
 version = "0.1.0"
+authors = ["Fredrik Bagge Carlson"]
 
 [deps]
+ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
 DyadControlSystems = "6f9e59d0-2838-4fee-9bce-c163a6c9592b"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
+OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 OrdinaryDiffEqDefault = "50262376-6c5a-4cf5-baba-aaf4f84d72d7"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
+
 
 [compat]
 ControlSystemsMTK = "2.5.0"
 ModelingToolkitStandardLibrary = "2.25.0"
 Plots = "1.41.2"
+StaticArrays = "1.9.15"
 
 [extras]
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

src/DiscreteKalmanFilter.jl

@@ -1,4 +1,5 @@
 using StaticArrays
+export IMUKalmanFilter, predict!, correct!, update!, compute_angles, bias, angle
 
 # System: x = [angle; bias], dynamics: ẋ = A_c*x + B_c*u, measurement: y = C*x
 # Continuous: A_c = [0 -1; 0 0], B_c = [1; 0]
@@ -11,75 +12,72 @@ const Cd = @SMatrix [1.0f0 0.0f0]             # Measurement matrix (1×2)
 const I2 = @SMatrix [1.0f0 0.0f0; 0.0f0 1.0f0]    # Identity for covariance update
 
 """
-    DiscreteKalmanFilter
+    IMUKalmanFilter
 
 Discrete-time Kalman filter for angle estimation from gyro (input) and accelerometer (measurement).
 State is `[angle, gyro_bias]`.
 """
-mutable struct DiscreteKalmanFilter
+mutable struct IMUKalmanFilter
     x::SVector{2, Float32}              # State [angle, bias]
     P::SMatrix{2, 2, Float32, 4}        # Covariance
     const Q::SMatrix{2, 2, Float32, 4}        # Process noise covariance
     const R::SMatrix{1, 1, Float32, 1}        # Measurement noise covariance
 end
 
 """
-    DiscreteKalmanFilter(; Q_angle=0.001f0, Q_bias=0.005f0, R_angle=0.5)
+    IMUKalmanFilter(; Q_angle=0.001f0, Q_bias=0.005f0, R_angle=0.5)
 
 Create a discrete Kalman filter with specified noise covariances.
 Default values from BalanceCar.h.
 """
-function DiscreteKalmanFilter(; Q_angle::Float32=0.001f0, Q_bias::Float32=0.005f0, R_angle::Float32=0.5f0)
+function IMUKalmanFilter(; Q_angle::Float32=0.001f0, Q_bias::Float32=0.005f0, R_angle::Float32=0.5f0)
     x = @SVector zeros(Float32, 2)
     P = I2
     Q = @SMatrix [Q_angle 0.0f0; 0.0f0 Q_bias]
     R = @SMatrix [R_angle;;]
-    DiscreteKalmanFilter(x, P, Q, R)
+    IMUKalmanFilter(x, P, Q, R)
 end
 
 """
     predict!(kf, u)
 
-Prediction step: propagate state and covariance using gyro measurement `u` as input.
-
-    x = A * x + B * u
-    P = A * P * A' + Q
+Prediction step: propagate state and covariance using gyro tilt rate (x-axis) measurement `u` as input.
 """
-@inline @fastmath function predict!(kf::DiscreteKalmanFilter, u::Float32)
-    kf.x = Ad * kf.x + Bd * u
-    kf.P = Ad * kf.P * Ad' + kf.Q
+@inline @fastmath function predict!(kf::IMUKalmanFilter, u::Float32)
+    kf.x = Ad*kf.x + Bd*u
+    kf.P = Ad*kf.P*Ad' + kf.Q
     nothing
 end
 
 """
     correct!(kf, y)
 
 Update step: correct state and covariance using accelerometer angle measurement `y`.
-
-    S = C * P * C' + R
-    K = P * C' / S
-    x = x + K * (y - C * x)
-    P = (I - K * C) * P
 """
-@inline @fastmath function correct!(kf::DiscreteKalmanFilter, y::Float32)
+@inline @fastmath function correct!(kf::IMUKalmanFilter, y::Float32)
     # Innovation covariance (1×1 matrix)
-    S = Cd * kf.P * Cd' + kf.R
+    S = Cd*kf.P*Cd' + kf.R
 
     # Kalman gain (2×1)
-    K = kf.P * Cd' / S[1, 1]
+    K = kf.P*Cd' / S[1, 1]
 
     # Innovation (scalar wrapped as 1×1 for matrix multiply)
-    innovation = SA[y] - Cd * kf.x
+    innovation = SA[y] - Cd*kf.x
 
     # State update
-    kf.x = kf.x + K * innovation
+    kf.x = kf.x + K*innovation
 
     # Covariance update
-    kf.P = (I2 - K * Cd) * kf.P
+    kf.P = symmetrize((I2 - K * Cd) * kf.P)
 
     nothing
 end
 
+function symmetrize(P)
+    m = P[2,1] + P[1,2]
+    SA[P[1,1] m; m P[2,2]]
+end
+
 """
     update!(kf, u, y)
 
@@ -88,7 +86,7 @@ Combined predict + correct step.
 - `u`: gyro measurement (angular velocity)
 - `y`: accelerometer angle measurement
 """
-function update!(kf::DiscreteKalmanFilter, u::Float32, y::Float32)
+function update!(kf::IMUKalmanFilter, u::Float32, y::Float32)
     predict!(kf, u)
     correct!(kf, y)
     nothing
@@ -99,14 +97,14 @@ end
 
 Get the estimated angle from the filter state.
 """
-angle(kf::DiscreteKalmanFilter) = kf.x[1]
+angle(kf::IMUKalmanFilter) = kf.x[1]
 
 """
     bias(kf)
 
 Get the estimated gyro bias from the filter state.
 """
-bias(kf::DiscreteKalmanFilter) = kf.x[2]
+bias(kf::IMUKalmanFilter) = kf.x[2]
 
 # IMU calibration constants (from original C++ code)
 const GYRO_OFFSET = 128.1f0
@@ -117,7 +115,7 @@ const GYRO_SCALE = 131.0f0
 
 Apply calibration to raw IMU readings and compute
 - `angle`: Tilt angle from accelerometer (radians)
-- `gyro_x`: Calibrated gyro x (angular velocity) for tile derivative control
+- `gyro_x`: Calibrated gyro x (angular velocity) for tilt derivative control
 - `gyro_z`: Calibrated gyro z (angular velocity) for turn derivative control
 
 Applies calibration to gyro readings and computes angle from accelerometer.
@@ -131,11 +129,11 @@ That is, the gyro x reading is used as control input, and the angle computed fro
 
 
 # Arguments
-- `kf`: DiscreteKalmanFilter instance
+- `kf`: IMUKalmanFilter instance
 - `ax, ay, az`: Raw accelerometer readings (int16)
 - `gx, gy, gz`: Raw gyroscope readings (int16)
 """
-function compute_angles(kf::DiscreteKalmanFilter, ax::Integer, ay::Integer, az::Integer,
+function compute_angles(kf::IMUKalmanFilter, ax::Integer, ay::Integer, az::Integer,
                        gx::Integer, gy::Integer, gz::Integer)
     # Calculate angle from accelerometer (radians to degrees)
     angle = atan(ay, az) * (180.0f0 / pi)
@@ -150,13 +148,3 @@ function compute_angles(kf::DiscreteKalmanFilter, ax::Integer, ay::Integer, az::
     angle, gyro_x, gyro_z
 end
 
-
-
-# test
-f = DiscreteKalmanFilter()
-
-m_gyro_x = 0.1f0  # rad/s
-m_angle = 0.05f0  # rad
-update!(f, m_gyro_x, m_angle)
-@show angle(f)
-@show bias(f)

src/DyadBotComponents.jl

@@ -1,6 +1,8 @@
 module DyadBotComponents
 
-include("planar_multibody.jl")
+include("DiscreteKalmanFilter.jl")
+
+# include("planar_multibody.jl")
 # include("segway_3d.jl")
 
 end # module DyadBotComponents

test/runtests.jl

@@ -0,0 +1,25 @@
+
+using DyadBotComponents
+using Test
+    
+# include("../generated/tests.jl")
+
+# test
+f = IMUKalmanFilter()
+
+m_gyro_x = 0.0f0  # rad/s
+m_angle = 0.05f0  # rad
+for i = 1:100
+    update!(f, m_gyro_x, m_angle)
+end
+@test angle(f) ≈ m_angle rtol = 1e-4
+@test abs(bias(f)) < 1e-4
+
+
+m_gyro_x = 0.1f0  # rad/s
+m_angle = 0.05f0  # rad
+for i = 1:100
+    update!(f, m_gyro_x, m_angle)
+end
+@test angle(f) ≈ m_angle rtol = 1e-4
+@test bias(f) ≈ 0.1 rtol = 1e-4