update the vel calculation.
the integration should only executed in world frame, not in imu body frame
This commit is contained in:
LiuzhengSJ
75
get_vel.py
Normal file
75
get_vel.py
Normal file
@ -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)
|
||||
|
||||
329
vel_esti.py
329
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))
|
||||
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)
|
||||
Reference in New Issue
Block a user