From 20a6f599426c219045660c94d462209b21795fdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=8C=82=E4=B9=8B=E9=92=B3?= Date: Wed, 27 May 2026 15:23:04 +0000 Subject: [PATCH] fix: coordinate transform from Android ENU to user NED frame User frame: X=forward (phone top), Y=right, Z=down Pitch positive when lifting phone top, roll positive when right side up, yaw positive when turning right (clockwise). Conversion applied in initializeFromSensors, predict (gyro), and update: X_user = Y_android Y_user = X_android Z_user = -Z_android rotX_user = rotY_android, rotY_user = rotX_android, rotZ_user = -rotZ_android --- .../com/senstools/domain/ekf/EKFAttitude.kt | 121 ++++++++++-------- 1 file changed, 67 insertions(+), 54 deletions(-) diff --git a/android/app/src/main/java/com/senstools/domain/ekf/EKFAttitude.kt b/android/app/src/main/java/com/senstools/domain/ekf/EKFAttitude.kt index 470c57e..4baf36a 100644 --- a/android/app/src/main/java/com/senstools/domain/ekf/EKFAttitude.kt +++ b/android/app/src/main/java/com/senstools/domain/ekf/EKFAttitude.kt @@ -92,13 +92,15 @@ class EKFAttitude { * Initialize attitude from accelerometer and magnetometer. * This provides a good initial guess before the EKF converges. */ - fun initializeFromSensors(accelerometer: FloatArray, magnetometer: FloatArray) { - val ax = accelerometer[0] - val ay = accelerometer[1] - val az = accelerometer[2] - val mx = magnetometer[0] - val my = magnetometer[1] - val mz = magnetometer[2] + fun initializeFromSensors(accelerometerIn: FloatArray, magnetometerIn: FloatArray) { + // Android sensor frame (X=right, Y=up/forward, Z=out) to + // user frame (X=forward/top, Y=right, Z=down): + val ax = accelerometerIn[1] + val ay = accelerometerIn[0] + val az = -accelerometerIn[2] + val mx = magnetometerIn[1] + val my = magnetometerIn[0] + val mz = -magnetometerIn[2] // Roll and pitch from accelerometer val roll = atan2(ay.toDouble(), az.toDouble()).toFloat() @@ -140,7 +142,7 @@ class EKFAttitude { * @param gyroscope angular velocity in rad/s [gx, gy, gz] * @param timestamp current timestamp in milliseconds */ - fun predict(gyroscope: FloatArray, timestamp: Long) { + fun predict(gyroscopeIn: FloatArray, timestamp: Long) { if (!initialized) { lastTimestamp = timestamp initialized = true @@ -154,9 +156,14 @@ class EKFAttitude { } lastTimestamp = timestamp - val gx = gyroscope[0] - val gy = gyroscope[1] - val gz = gyroscope[2] + // Convert gyroscope from Android frame (X=right, Y=up, Z=out) to + // user frame (X=forward, Y=right, Z=down): + // rotX_user = rotY_android + // rotY_user = rotX_android + // rotZ_user = -rotZ_android + val gx = gyroscopeIn[1] // roll rate (around X_user = Android Y) + val gy = gyroscopeIn[0] // pitch rate (around Y_user = Android X) + val gz = -gyroscopeIn[2] // yaw rate (around Z_user = -Android Z) // Quaternion derivative: dq/dt = 0.5 * q * omega // Where omega = [0, gx, gy, gz] @@ -229,7 +236,19 @@ class EKFAttitude { * @param accelerometer in m/s² [ax, ay, az] * @param magnetometer in µT [mx, my, mz] */ - fun update(accelerometer: FloatArray, magnetometer: FloatArray) { + fun update(accelerometerIn: FloatArray, magnetometerIn: FloatArray) { + // Convert from Android sensor frame (X=right, Y=up/forward, Z=out) to + // requested frame (X=forward/top, Y=right, Z=down): + // X_user = Y_android + // Y_user = X_android + // Z_user = -Z_android + val ax = accelerometerIn[1] + val ay = accelerometerIn[0] + val az = -accelerometerIn[2] + val mx = magnetometerIn[1] + val my = magnetometerIn[0] + val mz = -magnetometerIn[2] + val q0 = x[0] val q1 = x[1] val q2 = x[2] @@ -238,14 +257,12 @@ class EKFAttitude { // --- Accelerometer measurement model --- // Expected gravity in body frame: h_acc = [2*(q1*q3 - q0*q2), 2*(q0*q1 + q2*q3), q0^2 - q1^2 - q2^2 + q3^2] val normAccel = sqrt( - (accelerometer[0] * accelerometer[0] + - accelerometer[1] * accelerometer[1] + - accelerometer[2] * accelerometer[2]).toDouble() + (ax * ax + ay * ay + az * az).toDouble() ).toFloat() - val ax = if (normAccel > 1e-6f) accelerometer[0] / normAccel else 0f - val ay = if (normAccel > 1e-6f) accelerometer[1] / normAccel else 0f - val az = if (normAccel > 1e-6f) accelerometer[2] / normAccel else 0f + val axN = if (normAccel > 1e-6f) ax / normAccel else 0f + val ayN = if (normAccel > 1e-6f) ay / normAccel else 0f + val azN = if (normAccel > 1e-6f) az / normAccel else 0f // Expected gravity direction val hx_acc = 2f * (q1 * q3 - q0 * q2) @@ -253,29 +270,25 @@ class EKFAttitude { val hz_acc = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3 // --- Magnetometer measurement model --- - // Normalize magnetometer readings - val normMag = sqrt( - (magnetometer[0] * magnetometer[0] + - magnetometer[1] * magnetometer[1] + - magnetometer[2] * magnetometer[2]).toDouble() - ).toFloat() + // Normalize magnetometer readings (already in user frame from above) + val normMag = sqrt((mx * mx + my * my + mz * mz).toDouble()).toFloat() - val mx = if (normMag > 1e-6f) magnetometer[0] / normMag else 0f - val my = if (normMag > 1e-6f) magnetometer[1] / normMag else 0f - val mz = if (normMag > 1e-6f) magnetometer[2] / normMag else 0f + val mxN = if (normMag > 1e-6f) mx / normMag else 0f + val myN = if (normMag > 1e-6f) my / normMag else 0f + val mzN = if (normMag > 1e-6f) mz / normMag else 0f // Expected magnetic field in body frame // h_mag = R(q) * [Bx, 0, Bz] where Bx, Bz are local magnetic components // Simplified: we use the standard rotation matrix - val hx_mag = 2f * (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * mx + - 2f * (q1 * q2 - q0 * q3) * my + - 2f * (q1 * q3 + q0 * q2) * mz - val hy_mag = 2f * (q1 * q2 + q0 * q3) * mx + - 2f * (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * my + - 2f * (q2 * q3 - q0 * q1) * mz - val hz_mag = 2f * (q1 * q3 - q0 * q2) * mx + - 2f * (q2 * q3 + q0 * q1) * my + - 2f * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * mz + val hx_mag = 2f * (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * mxN + + 2f * (q1 * q2 - q0 * q3) * myN + + 2f * (q1 * q3 + q0 * q2) * mzN + val hy_mag = 2f * (q1 * q2 + q0 * q3) * mxN + + 2f * (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * myN + + 2f * (q2 * q3 - q0 * q1) * mzN + val hz_mag = 2f * (q1 * q3 - q0 * q2) * mxN + + 2f * (q2 * q3 + q0 * q1) * myN + + 2f * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * mzN // --- Jacobian H (6x4) --- val H = Array(6) { FloatArray(4) } @@ -297,29 +310,29 @@ class EKFAttitude { H[2][3] = 2f * q3 // Magnetometer Jacobian (simplified) - H[3][0] = 2f * q0 * mx + 2f * q3 * my - 2f * q2 * mz - H[3][1] = 2f * q1 * mx + 2f * q2 * my + 2f * q3 * mz - H[3][2] = -2f * q2 * mx + 2f * q1 * my + 2f * q0 * mz - H[3][3] = -2f * q3 * mx - 2f * q0 * my + 2f * q1 * mz + H[3][0] = 2f * q0 * mxN + 2f * q3 * myN - 2f * q2 * mzN + H[3][1] = 2f * q1 * mxN + 2f * q2 * myN + 2f * q3 * mzN + H[3][2] = -2f * q2 * mxN + 2f * q1 * myN + 2f * q0 * mzN + H[3][3] = -2f * q3 * mxN - 2f * q0 * myN + 2f * q1 * mzN - H[4][0] = -2f * q3 * mx + 2f * q0 * my + 2f * q1 * mz - H[4][1] = 2f * q2 * mx + 2f * q1 * my - 2f * q0 * mz - H[4][2] = 2f * q1 * mx - 2f * q2 * my + 2f * q3 * mz - H[4][3] = 2f * q0 * mx + 2f * q3 * my - 2f * q2 * mz + H[4][0] = -2f * q3 * mxN + 2f * q0 * myN + 2f * q1 * mzN + H[4][1] = 2f * q2 * mxN + 2f * q1 * myN - 2f * q0 * mzN + H[4][2] = 2f * q1 * mxN - 2f * q2 * myN + 2f * q3 * mzN + H[4][3] = 2f * q0 * mxN + 2f * q3 * myN - 2f * q2 * mzN - H[5][0] = 2f * q2 * mx - 2f * q1 * my + 2f * q0 * mz - H[5][1] = 2f * q3 * mx + 2f * q0 * my + 2f * q1 * mz - H[5][2] = 2f * q0 * mx - 2f * q3 * my + 2f * q2 * mz - H[5][3] = -2f * q1 * mx + 2f * q2 * my + 2f * q3 * mz + H[5][0] = 2f * q2 * mxN - 2f * q1 * myN + 2f * q0 * mzN + H[5][1] = 2f * q3 * mxN + 2f * q0 * myN + 2f * q1 * mzN + H[5][2] = 2f * q0 * mxN - 2f * q3 * myN + 2f * q2 * mzN + H[5][3] = -2f * q1 * mxN + 2f * q2 * myN + 2f * q3 * mzN // --- Innovation (measurement residual) --- val z = FloatArray(6) - z[0] = ax - hx_acc - z[1] = ay - hy_acc - z[2] = az - hz_acc - z[3] = mx - hx_mag - z[4] = my - hy_mag - z[5] = mz - hz_mag + z[0] = axN - hx_acc + z[1] = ayN - hy_acc + z[2] = azN - hz_acc + z[3] = mxN - hx_mag + z[4] = myN - hy_mag + z[5] = mzN - hz_mag // --- Kalman update --- // S = H * P * H^T + R