Skip to content

Commit 963cf06

Browse files
BenChungBenChung
committed
First go
1 parent 3ac39e5 commit 963cf06

File tree

7 files changed

+1746
-2
lines changed

7 files changed

+1746
-2
lines changed

‎deploy/Manifest.toml‎

Lines changed: 211 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,223 @@
11
# This file is machine-generated - editing it directly is not advised
22

3-
julia_version = "1.12.1"
3+
julia_version = "1.12.4"
44
manifest_format = "2.0"
5-
project_hash = "2e6b20c0f0eb37e1fc3d6652abaa16f2bc461506"
5+
project_hash = "ffb87b643daea33674fe3b248d832b13237c53d3"
6+
7+
[[deps.ArgTools]]
8+
uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f"
9+
version = "1.1.2"
10+
11+
[[deps.Artifacts]]
12+
uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33"
13+
version = "1.11.0"
14+
15+
[[deps.Base64]]
16+
uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
17+
version = "1.11.0"
18+
19+
[[deps.CompilerSupportLibraries_jll]]
20+
deps = ["Artifacts", "Libdl"]
21+
uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae"
22+
version = "1.3.0+1"
23+
24+
[[deps.Dates]]
25+
deps = ["Printf"]
26+
uuid = "ade2ca70-3891-5945-98fb-dc099432e06a"
27+
version = "1.11.0"
28+
29+
[[deps.Downloads]]
30+
deps = ["ArgTools", "FileWatching", "LibCURL", "NetworkOptions"]
31+
uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6"
32+
version = "1.7.0"
33+
34+
[[deps.FileWatching]]
35+
uuid = "7b1f6079-737a-58dc-b8bc-7a2ca5c1b5ee"
36+
version = "1.11.0"
37+
38+
[[deps.JuliaSyntaxHighlighting]]
39+
deps = ["StyledStrings"]
40+
uuid = "ac6e5ff7-fb65-4e79-a425-ec3bc9c03011"
41+
version = "1.12.0"
42+
43+
[[deps.LibCURL]]
44+
deps = ["LibCURL_jll", "MozillaCACerts_jll"]
45+
uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21"
46+
version = "0.6.4"
47+
48+
[[deps.LibCURL_jll]]
49+
deps = ["Artifacts", "LibSSH2_jll", "Libdl", "OpenSSL_jll", "Zlib_jll", "nghttp2_jll"]
50+
uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0"
51+
version = "8.15.0+0"
52+
53+
[[deps.LibGit2]]
54+
deps = ["LibGit2_jll", "NetworkOptions", "Printf", "SHA"]
55+
uuid = "76f85450-5226-5b5a-8eaa-529ad045b433"
56+
version = "1.11.0"
57+
58+
[[deps.LibGit2_jll]]
59+
deps = ["Artifacts", "LibSSH2_jll", "Libdl", "OpenSSL_jll"]
60+
uuid = "e37daf67-58a4-590a-8e99-b0245dd2ffc5"
61+
version = "1.9.0+0"
62+
63+
[[deps.LibSSH2_jll]]
64+
deps = ["Artifacts", "Libdl", "OpenSSL_jll"]
65+
uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8"
66+
version = "1.11.3+1"
67+
68+
[[deps.Libdl]]
69+
uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb"
70+
version = "1.11.0"
71+
72+
[[deps.LinearAlgebra]]
73+
deps = ["Libdl", "OpenBLAS_jll", "libblastrampoline_jll"]
74+
uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
75+
version = "1.12.0"
76+
77+
[[deps.Logging]]
78+
uuid = "56ddb016-857b-54e1-b83d-db4d58db5568"
79+
version = "1.11.0"
80+
81+
[[deps.Markdown]]
82+
deps = ["Base64", "JuliaSyntaxHighlighting", "StyledStrings"]
83+
uuid = "d6f4376e-aef5-505a-96c1-9c027394607a"
84+
version = "1.11.0"
85+
86+
[[deps.MozillaCACerts_jll]]
87+
uuid = "14a3606d-f60d-562e-9121-12d972cd8159"
88+
version = "2025.11.4"
89+
90+
[[deps.NetworkOptions]]
91+
uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908"
92+
version = "1.3.0"
93+
94+
[[deps.OpenBLAS_jll]]
95+
deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"]
96+
uuid = "4536629a-c528-5b80-bd46-f80d51c5b363"
97+
version = "0.3.29+0"
98+
99+
[[deps.OpenSSL_jll]]
100+
deps = ["Artifacts", "Libdl"]
101+
uuid = "458c3c95-2e84-50aa-8efc-19380b2a3a95"
102+
version = "3.5.4+0"
103+
104+
[[deps.PiGPIO]]
105+
deps = ["Sockets"]
106+
path = "dev/PiGPIO"
107+
uuid = "bb151fc1-c6dc-5496-8ed6-07f94907e623"
108+
version = "0.2.1"
109+
110+
[[deps.Pkg]]
111+
deps = ["Artifacts", "Dates", "Downloads", "FileWatching", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "Random", "SHA", "TOML", "Tar", "UUIDs", "p7zip_jll"]
112+
uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
113+
version = "1.12.1"
114+
115+
[deps.Pkg.extensions]
116+
REPLExt = "REPL"
117+
118+
[deps.Pkg.weakdeps]
119+
REPL = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb"
120+
121+
[[deps.PrecompileTools]]
122+
deps = ["Preferences"]
123+
git-tree-sha1 = "07a921781cab75691315adc645096ed5e370cb77"
124+
uuid = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
125+
version = "1.3.3"
126+
127+
[[deps.Preferences]]
128+
deps = ["TOML"]
129+
git-tree-sha1 = "522f093a29b31a93e34eaea17ba055d850edea28"
130+
uuid = "21216c6a-2e73-6563-6e65-726566657250"
131+
version = "1.5.1"
6132

7133
[[deps.Printf]]
8134
deps = ["Unicode"]
9135
uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7"
10136
version = "1.11.0"
11137

138+
[[deps.Random]]
139+
deps = ["SHA"]
140+
uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
141+
version = "1.11.0"
142+
143+
[[deps.SHA]]
144+
uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce"
145+
version = "0.7.0"
146+
147+
[[deps.Sockets]]
148+
uuid = "6462fe0b-24de-5631-8697-dd941f90decc"
149+
version = "1.11.0"
150+
151+
[[deps.StaticArrays]]
152+
deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore"]
153+
git-tree-sha1 = "eee1b9ad8b29ef0d936e3ec9838c7ec089620308"
154+
uuid = "90137ffa-7385-5640-81b9-e52037218182"
155+
version = "1.9.16"
156+
157+
[deps.StaticArrays.extensions]
158+
StaticArraysChainRulesCoreExt = "ChainRulesCore"
159+
StaticArraysStatisticsExt = "Statistics"
160+
161+
[deps.StaticArrays.weakdeps]
162+
ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4"
163+
Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
164+
165+
[[deps.StaticArraysCore]]
166+
git-tree-sha1 = "6ab403037779dae8c514bad259f32a447262455a"
167+
uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c"
168+
version = "1.4.4"
169+
170+
[[deps.StyledStrings]]
171+
uuid = "f489334b-da3d-4c2e-b8f0-e476e12c162b"
172+
version = "1.11.0"
173+
174+
[[deps.TOML]]
175+
deps = ["Dates"]
176+
uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76"
177+
version = "1.0.3"
178+
179+
[[deps.Tar]]
180+
deps = ["ArgTools", "SHA"]
181+
uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e"
182+
version = "1.10.0"
183+
184+
[[deps.TestEnv]]
185+
deps = ["Pkg"]
186+
git-tree-sha1 = "09a25374d0c0ccdd2f0ada8ac64f6555f3a13b31"
187+
uuid = "1e6cf692-eddd-4d53-88a5-2d735e33781b"
188+
version = "1.103.1"
189+
190+
[[deps.Timers]]
191+
deps = ["Pkg", "TestEnv"]
192+
git-tree-sha1 = "fe7046d2b5bc1d31cde8fd19fad7c5506e3960b4"
193+
uuid = "21f18d07-b854-4dab-86f0-c15a3821819a"
194+
version = "0.1.5"
195+
196+
[[deps.UUIDs]]
197+
deps = ["Random", "SHA"]
198+
uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
199+
version = "1.11.0"
200+
12201
[[deps.Unicode]]
13202
uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5"
14203
version = "1.11.0"
204+
205+
[[deps.Zlib_jll]]
206+
deps = ["Libdl"]
207+
uuid = "83775a58-1f1d-513f-b197-d71354ab007a"
208+
version = "1.3.1+2"
209+
210+
[[deps.libblastrampoline_jll]]
211+
deps = ["Artifacts", "Libdl"]
212+
uuid = "8e850b90-86db-534c-a0d3-1478176c7d93"
213+
version = "5.15.0+0"
214+
215+
[[deps.nghttp2_jll]]
216+
deps = ["Artifacts", "Libdl"]
217+
uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d"
218+
version = "1.64.0+1"
219+
220+
[[deps.p7zip_jll]]
221+
deps = ["Artifacts", "CompilerSupportLibraries_jll", "Libdl"]
222+
uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0"
223+
version = "17.7.0+0"

‎deploy/Project.toml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,5 @@
11
[deps]
2+
PiGPIO = "bb151fc1-c6dc-5496-8ed6-07f94907e623"
23
Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
4+
StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
5+
Timers = "21f18d07-b854-4dab-86f0-c15a3821819a"

‎deploy/src/DiscreteKalmanFilter.jl‎

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
using StaticArrays
2+
export IMUKalmanFilter, predict!, correct!, update!, compute_angles, bias, angle
3+
4+
# System: x = [angle; bias], dynamics: ẋ = A_c*x + B_c*u, measurement: y = C*x
5+
# Continuous: A_c = [0 -1; 0 0], B_c = [1; 0]
6+
# Discretized with Ts = 0.005 (200 Hz)
7+
8+
const Ts = 0.005f0
9+
const Ad = @SMatrix [1.0f0 -Ts; 0.0f0 1.0f0] # exp(A_c * Ts), exact since A_c² = 0
10+
const Bd = @SVector [Ts, 0.0f0] # ∫₀ᵈᵗ exp(A_c*τ) B_c dτ
11+
const Cd = @SMatrix [1.0f0 0.0f0] # Measurement matrix (1×2)
12+
const I2 = @SMatrix [1.0f0 0.0f0; 0.0f0 1.0f0] # Identity for covariance update
13+
14+
"""
15+
IMUKalmanFilter
16+
17+
Discrete-time Kalman filter for angle estimation from gyro (input) and accelerometer (measurement).
18+
State is `[angle, gyro_bias]`.
19+
"""
20+
mutable struct IMUKalmanFilter
21+
x::SVector{2, Float32} # State [angle, bias]
22+
P::SMatrix{2, 2, Float32, 4} # Covariance
23+
const Q::SMatrix{2, 2, Float32, 4} # Process noise covariance
24+
const R::SMatrix{1, 1, Float32, 1} # Measurement noise covariance
25+
end
26+
27+
"""
28+
IMUKalmanFilter(; Q_angle=0.001f0, Q_bias=0.005f0, R_angle=0.5)
29+
30+
Create a discrete Kalman filter with specified noise covariances.
31+
Default values from BalanceCar.h.
32+
"""
33+
function IMUKalmanFilter(; Q_angle::Float32=0.001f0, Q_bias::Float32=0.005f0, R_angle::Float32=0.5f0)
34+
x = @SVector zeros(Float32, 2)
35+
P = I2
36+
Q = @SMatrix [Q_angle 0.0f0; 0.0f0 Q_bias]
37+
R = @SMatrix [R_angle;;]
38+
IMUKalmanFilter(x, P, Q, R)
39+
end
40+
41+
"""
42+
predict!(kf, u)
43+
44+
Prediction step: propagate state and covariance using gyro tilt rate (x-axis) measurement `u` as input.
45+
"""
46+
@inline @fastmath function predict!(kf::IMUKalmanFilter, u::Float32)
47+
kf.x = Ad*kf.x + Bd*u
48+
kf.P = Ad*kf.P*Ad' + kf.Q
49+
nothing
50+
end
51+
52+
"""
53+
correct!(kf, y)
54+
55+
Update step: correct state and covariance using accelerometer angle measurement `y`.
56+
"""
57+
@inline @fastmath function correct!(kf::IMUKalmanFilter, y::Float32)
58+
# Innovation covariance (1×1 matrix)
59+
S = Cd*kf.P*Cd' + kf.R
60+
61+
# Kalman gain (2×1)
62+
K = kf.P*Cd' / S[1, 1]
63+
64+
# Innovation (scalar wrapped as 1×1 for matrix multiply)
65+
innovation = SA[y] - Cd*kf.x
66+
67+
# State update
68+
kf.x = kf.x + K*innovation
69+
70+
# Covariance update
71+
kf.P = symmetrize((I2 - K * Cd) * kf.P)
72+
73+
nothing
74+
end
75+
76+
function symmetrize(P)
77+
m = P[2,1] + P[1,2]
78+
SA[P[1,1] m; m P[2,2]]
79+
end
80+
81+
"""
82+
update!(kf, u, y)
83+
84+
Combined predict + correct step.
85+
86+
- `u`: gyro measurement (angular velocity)
87+
- `y`: accelerometer angle measurement
88+
"""
89+
function update!(kf::IMUKalmanFilter, u::Float32, y::Float32)
90+
predict!(kf, u)
91+
correct!(kf, y)
92+
nothing
93+
end
94+
95+
"""
96+
angle(kf)
97+
98+
Get the estimated angle from the filter state.
99+
"""
100+
angle(kf::IMUKalmanFilter) = kf.x[1]
101+
102+
"""
103+
bias(kf)
104+
105+
Get the estimated gyro bias from the filter state.
106+
"""
107+
bias(kf::IMUKalmanFilter) = kf.x[2]
108+
109+
# IMU calibration constants (from original C++ code)
110+
const GYRO_OFFSET = 128.1f0
111+
const GYRO_SCALE = 131.0f0
112+
113+
"""
114+
angle, gyro_x, gyro_z = compute_angles(ax, ay, az, gx, gy, gz)
115+
116+
Apply calibration to raw IMU readings and compute
117+
- `angle`: Tilt angle from accelerometer (radians)
118+
- `gyro_x`: Calibrated gyro x (angular velocity) for tilt derivative control
119+
- `gyro_z`: Calibrated gyro z (angular velocity) for turn derivative control
120+
121+
Applies calibration to gyro readings and computes angle from accelerometer.
122+
123+
Use with Kalman filter like this:
124+
```julia
125+
angle, gyro_x, gyro_z = compute_angles(ax, ay, az, gx, gy, gz)
126+
update!(kf, gyro_x, angle)
127+
```
128+
That is, the gyro x reading is used as control input, and the angle computed from accelerometer as measurement.
129+
130+
131+
# Arguments
132+
- `kf`: IMUKalmanFilter instance
133+
- `ax, ay, az`: Raw accelerometer readings (int16)
134+
- `gx, gy, gz`: Raw gyroscope readings (int16)
135+
"""
136+
function compute_angles(ax::Integer, ay::Integer, az::Integer,
137+
gx::Integer, gy::Integer, gz::Integer)
138+
# Calculate angle from accelerometer (radians to degrees)
139+
angle = atan(Float32(ay), Float32(az)) * (180.0f0 / pi)
140+
141+
# Apply gyro calibration offset and scale
142+
gyro_x = (gx - GYRO_OFFSET) / GYRO_SCALE
143+
144+
145+
# Store z-axis gyro (for turn control)
146+
gyro_z = -gz / GYRO_SCALE
147+
148+
angle, gyro_x, gyro_z
149+
end
150+

0 commit comments

Comments
 (0)