1+ import rclpy
2+ from rclpy .node import Node
3+ from sensor_msgs .msg import Imu
4+ from geometry_msgs .msg import Quaternion
5+ import numpy as np
6+
7+
8+ def quat_mul (q , r ):
9+ """Quaternion multiplication q x r"""
10+ w1 , x1 , y1 , z1 = q
11+ w2 , x2 , y2 , z2 = r
12+ return np .array ([
13+ w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 ,
14+ w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 ,
15+ w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2 ,
16+ w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
17+ ])
18+
19+
20+ def quat_rotate (q , v ):
21+ """Rotate vector v by quaternion q."""
22+ q_conj = np .array ([q [0 ], - q [1 ], - q [2 ], - q [3 ]])
23+ v_quat = np .array ([0.0 , v [0 ], v [1 ], v [2 ]])
24+ return quat_mul (quat_mul (q , v_quat ), q_conj )[1 :]
25+
26+
27+ class MahonyFilter (Node ):
28+ def __init__ (self ):
29+ """
30+ Tunable params:
31+ kP : Proportional gain
32+ kI : Integral gain
33+
34+ Need to specify intial attitude estimates, biases and sampling time
35+
36+ Let Initial attitude be zero for at rest,
37+ Let biases be computed by taking average samples of IMU at rest
38+ then by taking the mean value
39+ The sampling time is the inverse of the IMU publishing rate
40+
41+ NOTE: Bias drifts over time
42+
43+ ros2 launch phidgets_spatial spatial-launch.py --> IMU publisher
44+ """
45+ super ().__init__ ('mahony_filter' )
46+
47+ self .q = np .array ([1.0 , 0.0 , 0.0 , 0.0 ]) # Initial quaternion
48+ self .bg = np .array ([0.0 , 0.0 , 0.0 ]) # Inital gyro bias
49+ self .kP = 1.0
50+ self .kI = 0.01
51+ self .integral_error = np .zeros (3 )
52+
53+ self .last_time = None
54+
55+ self .imu_sub = self .create_subscription (
56+ Imu ,
57+ '/imu/data_raw' ,
58+ self .imu_callback ,
59+ 50
60+ )
61+
62+ # Needs to be implemented later
63+ # self.heading_sub = self.create_subscription(
64+
65+ self .pub = self .create_publisher (
66+ Quaternion ,
67+ '/imu/quaternion' ,
68+ 50
69+ )
70+
71+ def imu_callback (self , msg ):
72+ """
73+ Gryo Model:
74+ w = w_hat _+ b_g + n_g
75+ w : measured angular velocity
76+ w_hat: true angular velocity
77+ bg(t) : gyro bias -> bg_dot = bg(t) ~ N(0, Qg)
78+ Qg : covariance mtx gyro bias noise
79+
80+ Accel Model:
81+ a = R.T(a_hat - g) + b_a + n_a
82+
83+ a : measured acceleration
84+ a_hat: true acceleration
85+ g : gravity vector
86+ R : orientation of the sensor wrt the world frame
87+ b_a(t): accel bias -> b_a_dot = b_a(t) ~ N(0, Qa)
88+ Qa : covariance mtx accel bias noise
89+
90+ Orientation of the system must be known from an outside source
91+
92+ https://nitinjsanket.github.io/tutorials/attitudeest/mahony.html
93+
94+ ---------------------------------------------------------------------------
95+
96+ Defining a coord axis:
97+
98+ Let I, W, B be the inertial, world, and body frames respectively.
99+
100+ The goal is to estimate (I->W)q -> q is a quaternion
101+
102+ https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/asl-dam/documents/lectures/robot_dynamics/RD2_Quaternions.pdf
103+
104+
105+ 1) Obtain sensor measurements
106+ I^omega_t, I^a_t be gyro, accel measurements resp. and I^ahat_t be normed
107+
108+ 2) Orientation error using accel measurments
109+
110+ Compute orientation error from previous estimate using accel measurements
111+
112+ v (W->I qhat_est,t) = [2*(q2q4 - q1q3),
113+ 2*(q1q2 + q3q4),
114+ (q1^2 - q2^2 - q3^2 + q4^2)]
115+
116+ e_t+1 = I^ahat_t+1 x v(W->I qhat_est,t) # for P
117+ ei_t+1 = ei_t + e_t+1 * dt # for I
118+
119+ dt is the time elapsed between samples
120+
121+ 3) Update gyro using PI compensations (Fusion)
122+
123+ I^omega_t+1 = I^omega_t+1 + kP*e_t+1 + kI*ei_t+1
124+
125+ 4) Orientation increment from Gyro
126+
127+ (I->W qdot_omega,t+1) = 1/2 * (W->I qhat_est,t) quat_mult [0, I^omega_t+1].T
128+
129+ 5) Numerical Integration
130+
131+ Compute orientation using numerical integration
132+
133+ (I->W q_est,t+1) = (I->W qhat_est,t) + (I->W qdot_omega,t+1) * dt
134+
135+ For each time step, repeat from step 1
136+ ---------------------------------------------------------------------------
137+
138+ """
139+
140+ # --- Compute dt ---
141+ if self .last_time is None :
142+ self .last_time = msg .header .stamp
143+ return
144+
145+ t = msg .header .stamp
146+ t_now = t .sec + t .nanosec * 1e-9
147+ t_prev = self .last_time .sec + self .last_time .nanosec * 1e-9
148+ dt = t_now - t_prev
149+ self .last_time = t
150+ if dt <= 0 :
151+ return
152+ if dt > 0.5 :
153+ return # Ignore large time gaps
154+
155+ # --- Read IMU ---
156+ # rad/s
157+ gyro = np .array ([msg .angular_velocity .x ,
158+ msg .angular_velocity .y ,
159+ msg .angular_velocity .z ]) - self .bg
160+
161+ # m/s^2
162+ accel = np .array ([msg .linear_acceleration .x ,
163+ msg .linear_acceleration .y ,
164+ msg .linear_acceleration .z ])
165+
166+ if np .linalg .norm (accel ) == 0 :
167+ return
168+
169+ accel = accel / np .linalg .norm (accel ) # Only ahat is used
170+
171+ # --- Compute orientation error ---
172+ v = quat_rotate (self .q , np .array ([0.0 , 0.0 , - 1.0 ])) # Gravity vector in body frame
173+
174+ e = np .cross (v , accel )
175+ self .integral_error += e * dt
176+
177+ # --- Update gyro measurements ---
178+ gyro_corrected = gyro + self .kP * e + self .kI * self .integral_error
179+
180+ # --- Orientation increment from gyro ---
181+ qdot_omega = 0.5 * quat_mul (
182+ self .q ,
183+ np .array ([0.0 , gyro_corrected [0 ], gyro_corrected [1 ], gyro_corrected [2 ]]) # .T 1-d Array
184+ )
185+
186+ # --- Numerical integration ---
187+ self .q = self .q + qdot_omega * dt # q_t+1
188+ self .q = self .q / np .linalg .norm (self .q ) # Normalize
189+ # Non-unit quaternions lead to errors
190+ # --- Publish quaternion ---
191+ quat_msg = Quaternion ()
192+ quat_msg .w = self .q [0 ]
193+ quat_msg .x = self .q [1 ]
194+ quat_msg .y = self .q [2 ]
195+ quat_msg .z = self .q [3 ]
196+ self .pub .publish (quat_msg )
197+
198+
199+ def main (args = None ):
200+ rclpy .init (args = args )
201+ node = MahonyFilter ()
202+ try :
203+ rclpy .spin (node )
204+ except KeyboardInterrupt :
205+ pass
206+ rclpy .shutdown ()
207+
208+
209+ if __name__ == '__main__' :
210+ main ()
0 commit comments