From 70fa3002a410357abd7ace67e7eb22934691cd80 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Fri, 3 Jul 2026 14:40:29 +0100 Subject: [PATCH] update the vel calculation. the integration should only executed in world frame, not in imu body frame --- get_vel.py | 75 ++++++++++++ vel_esti.py | 329 +++++++++++++++++++++++++++------------------------- 2 files changed, 243 insertions(+), 161 deletions(-) create mode 100644 get_vel.py diff --git a/get_vel.py b/get_vel.py new file mode 100644 index 0000000..6875567 --- /dev/null +++ b/get_vel.py @@ -0,0 +1,75 @@ +import math + + +G = 9.80665 + + +def mat_vec_mul(R, v): + return [ + R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2], + R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2], + R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2], + ] + + +def mat_transpose(R): + return [ + [R[0][0], R[1][0], R[2][0]], + [R[0][1], R[1][1], R[2][1]], + [R[0][2], R[1][2], R[2][2]], + ] + + +def rotation_matrix_body_to_world(rpy_deg): + """ + rpy_deg = [roll, pitch, yaw], degree + + Convention: + roll around X + pitch around Y + yaw around Z + + R_body_to_world = Rz(yaw) @ Ry(pitch) @ Rx(roll) + """ + + roll = math.radians(rpy_deg[0]) + pitch = math.radians(rpy_deg[1]) + yaw = math.radians(rpy_deg[2]) + + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + + return [ + [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], + [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], + [-sp, cp * sr, cp * cr], + ] + +##################################################################### + +dt = 0.02 + +rpy_deg = [10, 20, 30] + +linear_acc_ms2 = [3.2, 5.5 , 7.7] # linear acc in imu body frame + +vel_world = [0.0, 0.0, 0.0] # linear vel in world frame + + +# get direction cosine matrix +R_bw = rotation_matrix_body_to_world(rpy_deg) +R_wb = mat_transpose(R_bw) + + +# Convert acceleration body -> world +acc_world_mps2 = mat_vec_mul(R_bw, linear_acc_ms2) + +# Integrate velocity in world frame +vel_world[0] += acc_world_mps2[0] * dt +vel_world[1] += acc_world_mps2[1] * dt +vel_world[2] += acc_world_mps2[2] * dt + +# Convert velocity world -> current body +vel_body = mat_vec_mul(R_wb, vel_world) + diff --git a/vel_esti.py b/vel_esti.py index 8679c2c..d8ea8f3 100644 --- a/vel_esti.py +++ b/vel_esti.py @@ -1,12 +1,31 @@ + + +''' +calculate the velocity of the imu. + +The measured acc is in imu body frame. + +1. acc offset along each axis calculation. + - gravity mapped to imu body frame, according to rpy; + - subtract gravity items to get the pure acc along each axis; + - keep imu static, average the pure acc values, as the offset; + - calibrate the offset in following use. + +2. vel calculation. + - get calibrated acc in imu body frame; + - convert this calibrate acc to world frame; + - integtate this acc, get the vel in world frame; + - convert the vel back to imu body frame. +''' + + import math -from typing import List, Tuple -G = 9.80665 # m/s^2 +G = 9.80665 -def mat_vec_mul(R: List[List[float]], v: List[float]) -> List[float]: - """3x3 matrix times 3x1 vector.""" +def mat_vec_mul(R, v): return [ R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2], R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2], @@ -14,8 +33,7 @@ def mat_vec_mul(R: List[List[float]], v: List[float]) -> List[float]: ] -def transpose(R: List[List[float]]) -> List[List[float]]: - """Transpose of 3x3 matrix.""" +def mat_transpose(R): return [ [R[0][0], R[1][0], R[2][0]], [R[0][1], R[1][1], R[2][1]], @@ -23,214 +41,203 @@ def transpose(R: List[List[float]]) -> List[List[float]]: ] -def rotation_matrix_body_to_world(rpy_deg: List[float]) -> List[List[float]]: +def rotation_matrix_body_to_world(rpy_deg): """ - Convert roll, pitch, yaw to rotation matrix. - - rpy_deg = [roll, pitch, yaw], unit degree + rpy_deg = [roll, pitch, yaw], degree Convention: - roll : rotation around X axis - pitch : rotation around Y axis - yaw : rotation around Z axis + roll around X + pitch around Y + yaw around Z - Rotation order: - R_body_to_world = Rz(yaw) @ Ry(pitch) @ Rx(roll) + R_body_to_world = Rz(yaw) @ Ry(pitch) @ Rx(roll) """ roll = math.radians(rpy_deg[0]) pitch = math.radians(rpy_deg[1]) yaw = math.radians(rpy_deg[2]) - cr = math.cos(roll) - sr = math.sin(roll) - - cp = math.cos(pitch) - sp = math.sin(pitch) - - cy = math.cos(yaw) - sy = math.sin(yaw) - - # R = Rz(yaw) @ Ry(pitch) @ Rx(roll) - R = [ - [ - cy * cp, - cy * sp * sr - sy * cr, - cy * sp * cr + sy * sr, - ], - [ - sy * cp, - sy * sp * sr + cy * cr, - sy * sp * cr - cy * sr, - ], - [ - -sp, - cp * sr, - cp * cr, - ], - ] - - return R - - -def gravity_body_from_rpy(rpy_deg: List[float]) -> List[float]: - """ - Calculate gravity vector projected onto body frame. - - Unit: g - - This matches your original formula: - g_x = -sin(pitch) - g_y = cos(pitch) * sin(roll) - g_z = cos(pitch) * cos(roll) - - Yaw does not affect gravity projection. - """ - - roll = math.radians(rpy_deg[0]) - pitch = math.radians(rpy_deg[1]) - - g_x = -math.sin(pitch) - g_y = math.cos(pitch) * math.sin(roll) - g_z = math.cos(pitch) * math.cos(roll) - - return [g_x, g_y, g_z] - - -def remove_gravity_in_body(acc_body_g: List[float], rpy_deg: List[float]) -> List[float]: - """ - Remove gravity directly in body frame. - - This is equivalent to your original acc_calib(). - Unit input : g - Unit output: g - """ - - g_body = gravity_body_from_rpy(rpy_deg) + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) return [ - acc_body_g[0] - g_body[0], - acc_body_g[1] - g_body[1], - acc_body_g[2] - g_body[2], + [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], + [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], + [-sp, cp * sr, cp * cr], ] class IMUVelocityEstimator: - """ - Estimate velocity from IMU acceleration. - - Internal velocity state is stored in world frame: - self.v_world = [vx, vy, vz], unit m/s - - Each update returns: - a_world_linear_mps2 : gravity-compensated linear acceleration in world frame - v_world : velocity in world frame - v_body : velocity projected onto current IMU/body XYZ axes - """ - def __init__(self): - self.v_world = [0.0, 0.0, 0.0] + # Velocity is stored in world frame, unit m/s + self.vel_world = [0.0, 0.0, 0.0] - def reset_velocity(self, v_world: List[float] = None): - if v_world is None: - self.v_world = [0.0, 0.0, 0.0] - else: - self.v_world = list(v_world) + # Acceleration offset in body frame, unit g + self.acc_offset_body = [0.0, 0.0, 0.0] - def update( - self, - acc_body_g: List[float], - rpy_deg: List[float], - dt: float, - ) -> Tuple[List[float], List[float], List[float]]: + def reset_velocity(self): + self.vel_world = [0.0, 0.0, 0.0] + + def set_acc_offset_body(self, acc_offset_body): + self.acc_offset_body = list(acc_offset_body) + + def update(self, acc_body_g, rpy_deg, dt): """ - One IMU update step. - Parameters ---------- acc_body_g: - IMU measured acceleration in current body frame. - Unit: g - Example: [Ax, Ay, Az] + measured acceleration in IMU/body frame, unit g rpy_deg: - IMU attitude angle. - Unit: degree - Format: [roll, pitch, yaw] + [roll, pitch, yaw], unit degree dt: - Sampling interval. - Unit: second + time step, unit second Returns ------- - a_world_linear_mps2: - Linear acceleration in world frame after gravity removal. - Unit: m/s^2 + acc_world_linear: + gravity-compensated acceleration in world frame, unit m/s^2 - v_world: - Integrated velocity in world frame. - Unit: m/s + vel_world: + velocity in world frame, unit m/s - v_body: - Current velocity projected onto current IMU/body XYZ axes. - Unit: m/s + vel_body: + velocity projected onto current IMU/body frame, unit m/s """ - # 1. Rotation matrix: body frame -> world frame R_bw = rotation_matrix_body_to_world(rpy_deg) + R_wb = mat_transpose(R_bw) - # 2. Convert acceleration from g to m/s^2 - acc_body_mps2 = [ - acc_body_g[0] * G, - acc_body_g[1] * G, - acc_body_g[2] * G, + # 1. Remove body-frame acceleration offset + acc_body_corrected_g = [ + acc_body_g[0] - self.acc_offset_body[0], + acc_body_g[1] - self.acc_offset_body[1], + acc_body_g[2] - self.acc_offset_body[2], ] - # 3. Transform acceleration from body frame to world frame + # 2. Convert body acceleration from g to m/s^2 + acc_body_mps2 = [ + acc_body_corrected_g[0] * G, + acc_body_corrected_g[1] * G, + acc_body_corrected_g[2] * G, + ] + + # 3. Convert acceleration body -> world acc_world_mps2 = mat_vec_mul(R_bw, acc_body_mps2) - # 4. Remove gravity in world frame - # - # Since your IMU flat on table gives Az ≈ +1g, - # and world Z is upward, gravity component in this convention is: - # - # g_world = [0, 0, +G] - # - # Therefore linear acceleration is: - # - # a_linear_world = acc_world - g_world - # - a_world_linear_mps2 = [ + # 4. Subtract gravity in world frame + # World Z is upward, and static flat IMU gives +1g on Z. + acc_world_linear = [ acc_world_mps2[0], acc_world_mps2[1], acc_world_mps2[2] - G, ] # 5. Integrate velocity in world frame - self.v_world[0] += a_world_linear_mps2[0] * dt - self.v_world[1] += a_world_linear_mps2[1] * dt - self.v_world[2] += a_world_linear_mps2[2] * dt + self.vel_world[0] += acc_world_linear[0] * dt + self.vel_world[1] += acc_world_linear[1] * dt + self.vel_world[2] += acc_world_linear[2] * dt - # 6. Transform world velocity back to current body frame - R_wb = transpose(R_bw) - v_body = mat_vec_mul(R_wb, self.v_world) + # 6. Convert velocity world -> current body + vel_body = mat_vec_mul(R_wb, self.vel_world) - return a_world_linear_mps2, list(self.v_world), v_body + return acc_world_linear, list(self.vel_world), vel_body + def calibrate_acc_offset(self, samples): + """ + Calibrate acceleration offset. + + Parameters + ---------- + samples: + list of samples. + Each sample should be: + (acc_body_g, rpy_deg) + + acc_body_g: [Ax, Ay, Az], unit g + rpy_deg: [roll, pitch, yaw], degree + + Principle + --------- + During static calibration: + + measured_acc_body = gravity_body + offset_body + + So: + + offset_body = measured_acc_body - gravity_body + + We average offset_body over all samples. + + Returns + ------- + acc_offset_body: + acceleration offset in body frame, unit g + """ + + if len(samples) == 0: + raise ValueError("samples is empty.") + + offset_sum = [0.0, 0.0, 0.0] + + for acc_body_g, rpy_deg in samples: + R_bw = rotation_matrix_body_to_world(rpy_deg) + R_wb = mat_transpose(R_bw) + + # Gravity in world frame. + # Unit g. + gravity_world_g = [0.0, 0.0, 1.0] + + # Convert gravity world -> body + gravity_body_g = mat_vec_mul(R_wb, gravity_world_g) + + # Offset = measured acceleration - expected gravity + offset_body_g = [ + acc_body_g[0] - gravity_body_g[0], + acc_body_g[1] - gravity_body_g[1], + acc_body_g[2] - gravity_body_g[2], + ] + + offset_sum[0] += offset_body_g[0] + offset_sum[1] += offset_body_g[1] + offset_sum[2] += offset_body_g[2] + + n = len(samples) + + self.acc_offset_body = [ + offset_sum[0] / n, + offset_sum[1] / n, + offset_sum[2] / n, + ] + + # Calibration means current velocity should be zero + self.reset_velocity() + + return list(self.acc_offset_body) if __name__ == "__main__": estimator = IMUVelocityEstimator() - acc = [0.2, 0.73, 0.66] # unit g - rpy = [46, -10, -8] # [roll, pitch, yaw], unit degree - dt = 0.01 # 100 Hz sample rate + # Example calibration samples. + # In real use, collect these while the IMU is static. + calib_samples = [ + ([0.2, 0.73, 0.66], [46, -10, -8]), + ([0.201, 0.731, 0.661], [46, -10, -8]), + ([0.199, 0.729, 0.659], [46, -10, -8]), + ] - a_world, v_world, v_body = estimator.update(acc, rpy, dt) + offset = estimator.calibrate_acc_offset(calib_samples) + print("acc_offset_body =", offset) - print("a_world_linear_mps2 =", a_world) - print("v_world_mps =", v_world) - print("v_body_mps =", v_body) + dt = 0.01 - # For comparison with your original direct body-frame gravity compensation: - print("a_body_linear_g =", remove_gravity_in_body(acc, rpy)) \ No newline at end of file + acc = [0.22, 0.75, 0.67] + rpy = [46, -10, -8] + + acc_world_linear, vel_world, vel_body = estimator.update(acc, rpy, dt) + + print("acc_world_linear =", acc_world_linear) + print("vel_world =", vel_world) + print("vel_body =", vel_body) \ No newline at end of file