x and y axes are zero-d out on my implementation. about fusion

[xioTechnologies]

I suspect that the x, y, z, and w elements of the quaternion do not mean what you think they mean. I suggest you instead convert the quaternion to Euler angles using the FusionQuaternionToEuler function, and use the roll, pitch, and yaw values.

[arjunsbalaji]

unfortunately I tried that but similarly only the yaw value is non zero. The ahrs class seems to be stuck in the initialising state which may be a part of my problem, have you ever come across that? The python version works perfectly, so I must just be doing something wrong. Here's my code below. I've also tried using it without magnetometer and the heading update but neither seem to work as I would like them too.

//
// SimSceneManager.swift
// pace
//
// Created by Arjun Balaji on 21/5/2022.
//

import Foundation
import SceneKit
import CoreMotion
import SwiftUI

class SimSceneManager: NSObject, SCNSceneRendererDelegate {

let scene = SCNScene()
var sensor: SCNNode!
var cameraNode: SCNNode!
var omniLightNode: SCNNode!
var ambiLightNode: SCNNode!

var motionManager: CMMotionManager!

var ax: Float = 0
var ay: Float = 0
var az: Float = 1

var omega_x: Float = 0
var omega_y: Float = 0
var omega_z: Float = 1

var Bx: Float = 1
var By: Float = 0
var Bz: Float = 0

var ahrs = FusionAhrs()
var offset = FusionOffset()

override init() {
    super.init()
    initScene()
}

func initScene() {
    initCamera()
    createSensor()
    initLights()
    //scene.isPaused = false
    
    motionManager = CMMotionManager()
    motionManager.startAccelerometerUpdates()
    motionManager.startGyroUpdates()
    motionManager.startMagnetometerUpdates()
    
    motionManager.startDeviceMotionUpdates()
    
    withUnsafeMutablePointer(to: &ahrs) { FusionAhrsInitialise($0) }
    withUnsafeMutablePointer(to: &ahrs) { FusionAhrsReset($0) }
    
    
}

func renderer(_ renderer: SCNSceneRenderer, updateAtTime time: TimeInterval) {
    
    let accelData = motionManager.accelerometerData!.acceleration
    let gyroData = motionManager.gyroData!.rotationRate
    let magneticField = motionManager.magnetometerData!.magneticField
    
    //let deviceMotionData = motionManager.deviceMotion!.attitude.quaternion
    
    
    ax = Float(accelData.x)
    ay = Float(accelData.y)
    az = Float(accelData.z)
    
    //change to degs a second
    omega_x = Float(gyroData.x)*180/(.pi)
    omega_y = Float(gyroData.y)*180/(.pi)
    omega_z = Float(gyroData.z)*180/(.pi)
    
    Bx = Float(magneticField.x)*1000
    By = Float(magneticField.y)*1000
    Bz = Float(magneticField.z)*1000
    
    print(omega_x, omega_y, omega_z)
    print(ax, ay, az)
    print(Bx, By, Bz)
    //print(ahrs.accelerometer.array)
    
    var gyroFusionVector = FusionVector(array: (omega_x, omega_y, omega_z))
    let accelFusionVector = FusionVector(array: (ax, ay, az))
    let magFusionVector = FusionVector(array: (Bx, By, Bz))

    gyroFusionVector = withUnsafeMutablePointer(to: &offset) {
        FusionOffsetUpdate($0, gyroFusionVector)
    }
    
    withUnsafeMutablePointer(to: &ahrs) {
        FusionAhrsUpdate($0,
                         gyroFusionVector,
                         accelFusionVector,
                         magFusionVector,
                         Float(1/60))
    }
    
    let eulerAngles = FusionQuaternionToEuler(ahrs.quaternion)
    
    print(eulerAngles.array)
    
    let sensor = scene.rootNode.childNode(withName: "sensor1", recursively: true)
    sensor?.orientation.x = Float(eulerAngles.angle.roll)
    sensor?.orientation.y = Float(eulerAngles.angle.pitch)
    sensor?.orientation.z = Float(eulerAngles.angle.yaw)
    
    //sensor?.orientation.x = Float(deviceMotionData.x)
    //sensor?.orientation.y = Float(deviceMotionData.y)
    //sensor?.orientation.z = Float(deviceMotionData.z)
    
}

func createSensor() {
    let sensorGeometery:SCNGeometry = SCNBox(width: 7,
                                             height: 25,
                                             length: 2,
                                             chamferRadius: CGFloat(1)) //cm.
    sensorGeometery.materials.first?.diffuse.contents = UIColor.white
    
    let sensor = SCNNode(geometry: sensorGeometery)
    sensor.name = "sensor1"
    scene.rootNode.addChildNode(sensor)
}

func initSensorData() {
}

func initLights() {
    omniLightNode = SCNNode()
    omniLightNode.light = SCNLight()
    omniLightNode.light!.type = SCNLight.LightType.omni
    omniLightNode.light!.color = UIColor(white: 0.75, alpha: 1.0)
    omniLightNode.position = SCNVector3Make(0, 5, 5)
    scene.rootNode.addChildNode(omniLightNode)
    
    ambiLightNode = SCNNode()
    ambiLightNode.light = SCNLight()
    ambiLightNode.light!.type = SCNLight.LightType.omni
    ambiLightNode.light!.color = UIColor(white: 0.75, alpha: 1.0)
    ambiLightNode.position = SCNVector3Make(-5, -5, -10)
    scene.rootNode.addChildNode(ambiLightNode)
}

func initCamera() {
    cameraNode = SCNNode()
    cameraNode.camera = SCNCamera()
    cameraNode.position = SCNVector3(0,7,10)
}

}

[xioTechnologies]

The repository includes working C and Python examples. I believe the problem is your implementation. I am not able to debug the code you have provided.

[roymacdonald]

Hi, thanks for sharing your code.
I also see that the algorithm is outputting zero for x and y in the earth acceleration. You can actually see it with your own python examples and the data you have included.
As well, I think that the gravity is not being removed from the linear acceleration.
Is there any fix for this?

[xioTechnologies]

Thank you for raising this issue. I have investigated and found that a rejectionTimeout setting value of zero will cause the algorithm to continuously enter an initialisation state. The fix has been merged as commit 625d7c9. The latest release, v1.0.3 includes this fix. The linear acceleration and earth acceleration values are now as expected.

[roymacdonald]

@xioTechnologies thanks.