hm/sensTools
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 6 Wiki Activity

fix: coordinate transform from Android ENU to user NED frame
Build APK / build (push) Failing after 1m31s
Details

Browse Source 
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
This commit is contained in:
茂之钳
2026-05-27 15:23:04 +00:00
parent 453efbcf6d
commit 20a6f59942
1 changed files with 67 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
android/app/src/main/java/com/senstools/domain/ekf/EKFAttitude.kt
+67 -54
View File
@@ -92,13 +92,15 @@ class EKFAttitude {
* Initialize attitude from accelerometer and magnetometer. * Initialize attitude from accelerometer and magnetometer.
* This provides a good initial guess before the EKF converges. * This provides a good initial guess before the EKF converges.
*/ */
fun initializeFromSensors(accelerometer: FloatArray, magnetometer: FloatArray) { fun initializeFromSensors(accelerometerIn: FloatArray, magnetometerIn: FloatArray) {
val ax = accelerometer[0] // Android sensor frame (X=right, Y=up/forward, Z=out) to
val ay = accelerometer[1] // user frame (X=forward/top, Y=right, Z=down):
val az = accelerometer[2] val ax = accelerometerIn[1]
val mx = magnetometer[0] val ay = accelerometerIn[0]
val my = magnetometer[1] val az = -accelerometerIn[2]
val mz = magnetometer[2] val mx = magnetometerIn[1]
val my = magnetometerIn[0]
val mz = -magnetometerIn[2]
// Roll and pitch from accelerometer // Roll and pitch from accelerometer
val roll = atan2(ay.toDouble(), az.toDouble()).toFloat() 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 gyroscope angular velocity in rad/s [gx, gy, gz]
* @param timestamp current timestamp in milliseconds * @param timestamp current timestamp in milliseconds
*/ */
fun predict(gyroscope: FloatArray, timestamp: Long) { fun predict(gyroscopeIn: FloatArray, timestamp: Long) {
if (!initialized) { if (!initialized) {
lastTimestamp = timestamp lastTimestamp = timestamp
initialized = true initialized = true
@@ -154,9 +156,14 @@ class EKFAttitude {
} }
lastTimestamp = timestamp lastTimestamp = timestamp
val gx = gyroscope[0] // Convert gyroscope from Android frame (X=right, Y=up, Z=out) to
val gy = gyroscope[1] // user frame (X=forward, Y=right, Z=down):
val gz = gyroscope[2] // 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 // Quaternion derivative: dq/dt = 0.5 * q * omega
// Where omega = [0, gx, gy, gz] // Where omega = [0, gx, gy, gz]
@@ -229,7 +236,19 @@ class EKFAttitude {
* @param accelerometer in m/s² [ax, ay, az] * @param accelerometer in m/s² [ax, ay, az]
* @param magnetometer in µT [mx, my, mz] * @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 q0 = x[0]
val q1 = x[1] val q1 = x[1]
val q2 = x[2] val q2 = x[2]
@@ -238,14 +257,12 @@ class EKFAttitude {
// --- Accelerometer measurement model --- // --- 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] // 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( val normAccel = sqrt(
(accelerometer[0] * accelerometer[0] + (ax * ax + ay * ay + az * az).toDouble()
accelerometer[1] * accelerometer[1] +
accelerometer[2] * accelerometer[2]).toDouble()
).toFloat() ).toFloat()
val ax = if (normAccel > 1e-6f) accelerometer[0] / normAccel else 0f val axN = if (normAccel > 1e-6f) ax / normAccel else 0f
val ay = if (normAccel > 1e-6f) accelerometer[1] / normAccel else 0f val ayN = if (normAccel > 1e-6f) ay / normAccel else 0f
val az = if (normAccel > 1e-6f) accelerometer[2] / normAccel else 0f val azN = if (normAccel > 1e-6f) az / normAccel else 0f
// Expected gravity direction // Expected gravity direction
val hx_acc = 2f * (q1 * q3 - q0 * q2) 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 val hz_acc = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
// --- Magnetometer measurement model --- // --- Magnetometer measurement model ---
// Normalize magnetometer readings // Normalize magnetometer readings (already in user frame from above)
val normMag = sqrt( val normMag = sqrt((mx * mx + my * my + mz * mz).toDouble()).toFloat()
(magnetometer[0] * magnetometer[0] +
magnetometer[1] * magnetometer[1] +
magnetometer[2] * magnetometer[2]).toDouble()
).toFloat()
val mx = if (normMag > 1e-6f) magnetometer[0] / normMag else 0f val mxN = if (normMag > 1e-6f) mx / normMag else 0f
val my = if (normMag > 1e-6f) magnetometer[1] / normMag else 0f val myN = if (normMag > 1e-6f) my / normMag else 0f
val mz = if (normMag > 1e-6f) magnetometer[2] / normMag else 0f val mzN = if (normMag > 1e-6f) mz / normMag else 0f
// Expected magnetic field in body frame // Expected magnetic field in body frame
// h_mag = R(q) * [Bx, 0, Bz] where Bx, Bz are local magnetic components // h_mag = R(q) * [Bx, 0, Bz] where Bx, Bz are local magnetic components
// Simplified: we use the standard rotation matrix // Simplified: we use the standard rotation matrix
val hx_mag = 2f * (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * mx + val hx_mag = 2f * (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * mxN +
2f * (q1 * q2 - q0 * q3) * my + 2f * (q1 * q2 - q0 * q3) * myN +
2f * (q1 * q3 + q0 * q2) * mz 2f * (q1 * q3 + q0 * q2) * mzN
val hy_mag = 2f * (q1 * q2 + q0 * q3) * mx + val hy_mag = 2f * (q1 * q2 + q0 * q3) * mxN +
2f * (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * my + 2f * (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * myN +
2f * (q2 * q3 - q0 * q1) * mz 2f * (q2 * q3 - q0 * q1) * mzN
val hz_mag = 2f * (q1 * q3 - q0 * q2) * mx + val hz_mag = 2f * (q1 * q3 - q0 * q2) * mxN +
2f * (q2 * q3 + q0 * q1) * my + 2f * (q2 * q3 + q0 * q1) * myN +
2f * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * mz 2f * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * mzN
// --- Jacobian H (6x4) --- // --- Jacobian H (6x4) ---
val H = Array(6) { FloatArray(4) } val H = Array(6) { FloatArray(4) }
@@ -297,29 +310,29 @@ class EKFAttitude {
H[2][3] = 2f * q3 H[2][3] = 2f * q3
// Magnetometer Jacobian (simplified) // Magnetometer Jacobian (simplified)
H[3][0] = 2f * q0 * mx + 2f * q3 * my - 2f * q2 * mz H[3][0] = 2f * q0 * mxN + 2f * q3 * myN - 2f * q2 * mzN
H[3][1] = 2f * q1 * mx + 2f * q2 * my + 2f * q3 * mz H[3][1] = 2f * q1 * mxN + 2f * q2 * myN + 2f * q3 * mzN
H[3][2] = -2f * q2 * mx + 2f * q1 * my + 2f * q0 * mz H[3][2] = -2f * q2 * mxN + 2f * q1 * myN + 2f * q0 * mzN
H[3][3] = -2f * q3 * mx - 2f * q0 * my + 2f * q1 * mz 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][0] = -2f * q3 * mxN + 2f * q0 * myN + 2f * q1 * mzN
H[4][1] = 2f * q2 * mx + 2f * q1 * my - 2f * q0 * mz H[4][1] = 2f * q2 * mxN + 2f * q1 * myN - 2f * q0 * mzN
H[4][2] = 2f * q1 * mx - 2f * q2 * my + 2f * q3 * mz H[4][2] = 2f * q1 * mxN - 2f * q2 * myN + 2f * q3 * mzN
H[4][3] = 2f * q0 * mx + 2f * q3 * my - 2f * q2 * mz 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][0] = 2f * q2 * mxN - 2f * q1 * myN + 2f * q0 * mzN
H[5][1] = 2f * q3 * mx + 2f * q0 * my + 2f * q1 * mz H[5][1] = 2f * q3 * mxN + 2f * q0 * myN + 2f * q1 * mzN
H[5][2] = 2f * q0 * mx - 2f * q3 * my + 2f * q2 * mz H[5][2] = 2f * q0 * mxN - 2f * q3 * myN + 2f * q2 * mzN
H[5][3] = -2f * q1 * mx + 2f * q2 * my + 2f * q3 * mz H[5][3] = -2f * q1 * mxN + 2f * q2 * myN + 2f * q3 * mzN
// --- Innovation (measurement residual) --- // --- Innovation (measurement residual) ---
val z = FloatArray(6) val z = FloatArray(6)
z[0] = ax - hx_acc z[0] = axN - hx_acc
z[1] = ay - hy_acc z[1] = ayN - hy_acc
z[2] = az - hz_acc z[2] = azN - hz_acc
z[3] = mx - hx_mag z[3] = mxN - hx_mag
z[4] = my - hy_mag z[4] = myN - hy_mag
z[5] = mz - hz_mag z[5] = mzN - hz_mag
// --- Kalman update --- // --- Kalman update ---
// S = H * P * H^T + R // S = H * P * H^T + R