89 lines
3.2 KiB
Rust
89 lines
3.2 KiB
Rust
use core::f32::consts::PI;
|
|
use nalgebra::ComplexField;
|
|
use nalgebra::RealField;
|
|
|
|
const G: f32 = 9.80665;
|
|
|
|
/// Complementary filter state
|
|
pub struct TiltEstimator {
|
|
roll: f32, // phi
|
|
pitch: f32, // theta
|
|
yaw: f32, // psi (integrated gyro yaw, will drift)
|
|
alpha: f32, // complementary weight for gyro integration (0..1)
|
|
}
|
|
|
|
impl TiltEstimator {
|
|
pub fn new(alpha: f32) -> Self {
|
|
Self { roll: 0.0, pitch: 0.0, yaw: 0.0, alpha }
|
|
}
|
|
|
|
/// dt in seconds, accel in m/s^2 body frame, gyro in rad/s body rates (gx, gy, gz)
|
|
/// body axes assumed: x forward, y right, z down
|
|
pub fn update(&mut self, dt: f32, accel: (f32, f32, f32), gyro: (f32, f32, f32)) {
|
|
let (ax, ay, az) = accel;
|
|
let (gx, gy, gz) = gyro;
|
|
|
|
// 1) accel-based tilt estimates (avoid NaN by small epsilon)
|
|
let eps = 1e-6;
|
|
let roll_acc = (ay).atan2(az.max(eps)); // phi = atan2(ay, az)
|
|
let pitch_acc = (-ax).atan2((ay*ay + az*az).sqrt().max(eps)); // theta = atan2(-ax, sqrt(ay^2+az^2))
|
|
|
|
// 2) integrate gyro for angles (body rates -> approx angle rates)
|
|
// Note: for small angles, roll_dot ≈ gx, pitch_dot ≈ gy, yaw_dot ≈ gz in body frame if axes aligned.
|
|
// For better accuracy you'd convert rates to inertial derivatives but this is common and simple.
|
|
let roll_from_gyro = self.roll + gx * dt;
|
|
let pitch_from_gyro = self.pitch + gy * dt;
|
|
let yaw_from_gyro = self.yaw + gz * dt;
|
|
|
|
// 3) complementary combine
|
|
let alpha = self.alpha;
|
|
self.roll = alpha * roll_from_gyro + (1.0 - alpha) * roll_acc;
|
|
self.pitch = alpha * pitch_from_gyro + (1.0 - alpha) * pitch_acc;
|
|
self.yaw = yaw_from_gyro; // we typically don't correct yaw with accel (no observability)
|
|
|
|
// normalize yaw to -pi..pi
|
|
if self.yaw > PI { self.yaw -= 2.0*PI; }
|
|
if self.yaw < -PI { self.yaw += 2.0*PI; }
|
|
}
|
|
|
|
pub fn angles(&self) -> (f32, f32, f32) {
|
|
(self.roll, self.pitch, self.yaw)
|
|
}
|
|
}
|
|
|
|
/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
|
|
pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
|
|
let (ax, ay, az) = body_acc;
|
|
|
|
let (sr, cr) = roll.sin_cos();
|
|
let (sp, cp) = pitch.sin_cos();
|
|
let (sy, cy) = yaw.sin_cos();
|
|
|
|
// Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
|
|
// multiply R * a_body
|
|
let r11 = cy*cp;
|
|
let r12 = cy*sp*sr - sy*cr;
|
|
let r13 = cy*sp*cr + sy*sr;
|
|
|
|
let r21 = sy*cp;
|
|
let r22 = sy*sp*sr + cy*cr;
|
|
let r23 = sy*sp*cr - cy*sr;
|
|
|
|
let r31 = -sp;
|
|
let r32 = cp*sr;
|
|
let r33 = cp*cr;
|
|
|
|
// a_nav = R * a_body
|
|
let ax_nav = r11*ax + r12*ay + r13*az; // east
|
|
let ay_nav = r21*ax + r22*ay + r23*az; // north
|
|
let az_nav = r31*ax + r32*ay + r33*az; // up
|
|
|
|
// remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
|
|
// If accel measured gravity as +9.8 on sensor when static and z down convention used above,
|
|
// then az_nav will include +g in 'up' direction; subtract G.
|
|
let ax_lin = ax_nav;
|
|
let ay_lin = ay_nav;
|
|
let az_lin = az_nav - G; // linear acceleration in up axis
|
|
|
|
(ax_lin, ay_lin, az_lin)
|
|
} |