【IOS】惯性导航详解（包含角度、加速度、修正方式的api分析）

参考文献

iPhone的惯性导航，基于步态。https://www.docin.com/p-811792664.html
Inertial Odometry on Handheld Smartphones: https://arxiv.org/pdf/1703.00154.pdf
惯性导航项目相关代码：https://github.com/topics/inertial-navigation-systems
useracceleration苹果官方文档：https://developer.apple.com/documentation/coremotion/cmdevicemotion/1616149-useracceleration
手机导航https://ieeexplore.ieee.org/document/6139701

give me example that use iOS api to get accelarate in xyz.
I use it in swift. print the acceleration of x y z.
make the data collection part in a class. and show it in CintentView using the data in class.

using a filter in the acceleration data

时间间隔获取

在iOS中进行积分时，您可以选择使用motionManager.accelerometerUpdateInterval或(motion.timestamp - lastLocation.timestamp)作为时间间隔。

motionManager.accelerometerUpdateInterval：

motionManager.accelerometerUpdateInterval是CMMotionManager类的属性，表示加速度计更新的时间间隔。
这个值是由您设置的，通常用于控制加速度计数据的采样率。
如果您使用加速度计数据进行积分，可以使用这个时间间隔来估计每个采样点的时间间隔。
例如，如果您的加速度计更新间隔为0.01秒，您可以将其作为每个采样点之间的时间间隔。
这种方法适用于简单的运动情况，但可能受到加速度计的噪声和误差的影响。

(motion.timestamp - lastLocation.timestamp)：

motion.timestamp是CMDeviceMotion对象的属性，表示获取数据的时间戳。
lastLocation.timestamp是上一个位置更新的时间戳，如果您使用位置数据进行积分，可以使用这个时间戳来计算时间间隔。
这种方法适用于使用位置数据进行积分的情况，例如计算行进距离。
请注意，这种方法要求您同时获取位置数据，并且需要在每个位置更新时记录时间戳

加速度获取

class MileMeter {
    let motionManager = CMMotionManager()
    var referenceAttitude: CMAttitude?

    func startMotionUpdates() {
        // 检查设备是否支持加速度计和陀螺仪
        guard motionManager.isAccelerometerAvailable, motionManager.isGyroAvailable else {
            print("设备不支持加速度计或陀螺仪")
            return
        }
        
        // 设置更新频率
        motionManager.accelerometerUpdateInterval = 1.0 / 100.0 
        
        // 启动加速度计和陀螺仪更新
        motionManager.startAccelerometerUpdates()
        motionManager.startGyroUpdates()
        
        // 获取初始参考姿态
        if let referenceAttitude = motionManager.deviceMotion?.attitude {
            self.referenceAttitude = referenceAttitude
        }
        
        // 处理加速度计数据
        motionManager.startDeviceMotionUpdates(to: .main) { (motionData, error) in
            guard let motionData = motionData else {
                print("无法获取加速度计数据: \(error?.localizedDescription ?? "")")
                return
            }
            // 获取用户加速度，而不是混合
            
            // 校准加速度计数据
//            if let referenceAttitude = self.referenceAttitude {
//                motionData.acceleration = motionData.acceleration.applying(referenceAttitude.rotationMatrix)
//            }
            
            // 进行距离估计
            let acceleration = motionData.userAcceleration   //用户对设备施加的加速度，而不包括重力的影响

            
            // 在此处可以使用估计的速度和位移数据进行进一步的处理
            
            print("加速度: \(acceleration)")
        }
    }
    
    func stopMotionUpdates() {
        motionManager.stopAccelerometerUpdates()
        motionManager.stopGyroUpdates()
        motionManager.stopDeviceMotionUpdates()
    }
}

在这里插入代码片

角度获取

陀螺仪有两种：var attitude: CMAttitude { get }和var rotationRate: CMRotationRate { get } ：

CMAttitude:绝对角度

CMAttitude represents the device’s orientation or attitude in space.
It provides information about the device’s pitch, roll, and yaw angles.
The attitude is expressed as a quaternion, which is a mathematical representation of orientation.
You can access the attitude using the attitude property of a CMMotionManager object.
Example usage: let attitude = motionManager.deviceMotion?.attitude

CMRotationRate:旋转角度

CMRotationRate represents the device’s rotation rate or angular velocity.
It provides information about the device’s rotation speed around each axis (x, y, and z).
The rotation rate is expressed in radians per second.
You can access the rotation rate using the rotationRate property of a CMMotionManager object.
Example usage: let rotationRate = motionManager.deviceMotion?.rotationRate

校准

坐标系变换

            // 校准加速度计数据
//            if let referenceAttitude = self.referenceAttitude {
//                motionData.acceleration = motionData.acceleration.applying(referenceAttitude.rotationMatrix)
//            }
            

低通滤波器

func lowPassFilter(_ x: Double) -> Double {
    if abs(x) < 0.01 {
        return 0
    } else {
        return x
    }
}

卡尔曼滤波

import Foundation

struct KalmanFilter {
    var state: Double
    var covariance: Double
    
    let processNoise: Double
    let measurementNoise: Double
    
    mutating func update(measurement: Double) {
        // Prediction step
        let predictedState = state
        let predictedCovariance = covariance + processNoise
        
        // Update step
        let kalmanGain = predictedCovariance / (predictedCovariance + measurementNoise)
        state = predictedState + kalmanGain * (measurement - predictedState)
        covariance = (1 - kalmanGain) * predictedCovariance
    }
}

// Example usage
var filter = KalmanFilter(state: 0, covariance: 1, processNoise: 0.1, measurementNoise: 1)

let measurements = [1.2, 1.4, 1.6, 1.8, 2.0]

for measurement in measurements {
    filter.update(measurement: measurement)
    print("Filtered measurement: \(filter.state)")
}

最终代码

class DistanceCalculator {
    private let motionManager = CMMotionManager()
    @Published var totalDistance: Double = 0.0
    @Published var lastLocation: CMDeviceMotion?
    @Published var ax: Double = 0.0 // 距离
    @Published var ay: Double = 0.0
    @Published var az: Double = 0.0
    @Published var acc_ax: Double = 0.0 // 加速度
    @Published var acc_ay: Double = 0.0
    @Published var acc_az: Double = 0.0
    @Published var m_acc_ax: Double = 0.0 // 修正后的加速度
    @Published var m_acc_ay: Double = 0.0
    @Published var m_acc_az: Double = 0.0
    @Published var m_dis_ax: Double = 0.0 // 修正后的路程
    @Published var m_dis_ay: Double = 0.0
    @Published var m_dis_az: Double = 0.0
    @Published var m_totalDistance: Double = 0.0
    
    init() {
        // 检查设备是否支持运动数据的获取
        guard motionManager.isDeviceMotionAvailable else {
            print("设备不支持运动数据")
            return
        }
        
        // 设置更新间隔 官方推荐100hz以上
        motionManager.deviceMotionUpdateInterval = 0.01
        
        // 开始获取设备运动数据
        motionManager.startDeviceMotionUpdates(to: .main) { [weak self] (motion, error) in
            guard let motion = motion else {
                return
            }
            
            if let lastLocation = self?.lastLocation {
                self?.acc_ax = motion.userAcceleration.x
                self?.acc_ay = motion.userAcceleration.y
                self?.acc_az = motion.userAcceleration.z
                let velocity_x = motion.userAcceleration.x * (motion.timestamp - lastLocation.timestamp)
                let velocity_y = motion.userAcceleration.y * (motion.timestamp - lastLocation.timestamp)
                let velocity_z = motion.userAcceleration.z * (motion.timestamp - lastLocation.timestamp)
                self?.ax = velocity_x
                self?.ay += velocity_y
                self?.az += velocity_z
                // sqrt 有问题？变成nan
                self?.totalDistance += abs(velocity_x*velocity_x)
//                self?.totalDistance += sqrt(velocity_x*velocity_x + velocity_y+velocity_y + velocity_z*velocity_z)
                print("欧式距离里程：\(self?.totalDistance ?? 0)") //如果可选类型的值为nil，空合运算符会返回它的右侧的默认值（在这里是0）
                print("加速度：\(motion.userAcceleration)")
                
                // 使用低通滤波器
                let _m_acc_x = lowPassFilter(motion.userAcceleration.x)
                self?.m_acc_ax = _m_acc_x
                let _m_acc_y = lowPassFilter(motion.userAcceleration.y)
                self?.m_acc_ay = _m_acc_y
                let _m_acc_z = lowPassFilter(motion.userAcceleration.z)
                self?.m_acc_az = _m_acc_z
                
                let m_velocity_x = _m_acc_x * (motion.timestamp - lastLocation.timestamp)
                let m_velocity_y = _m_acc_y * (motion.timestamp - lastLocation.timestamp)
                let m_velocity_z = _m_acc_z * (motion.timestamp - lastLocation.timestamp)
                self?.m_dis_ax = m_velocity_x
                self?.m_dis_ay += m_velocity_y
                self?.m_dis_az += m_velocity_z
                
                self?.m_totalDistance += abs(m_velocity_x)
                
            }
            
            self?.lastLocation = motion
        }
    }
}