Stable tracking of DWM module

NearbyDemo/ARViewContainer.swift

@@ -0,0 +1,104 @@
+import SwiftUI
+import RealityKit
+import ARKit
+import Combine
+
+struct ARViewContainer: UIViewRepresentable {
+    @ObservedObject var arManager: ARManager
+    @ObservedObject var estimator: AnchorEstimator
+
+    func makeUIView(context: Context) -> ARView {
+        let arView = ARView(frame: .zero, cameraMode: .ar, automaticallyConfigureSession: false)
+        arView.session = arManager.session
+
+        // Create the red sphere entity representing the UWB anchor
+        let sphereRadius: Float = 0.05
+        let mesh = MeshResource.generateSphere(radius: sphereRadius)
+        var material = UnlitMaterial()
+        material.color = .init(tint: .red)
+        let sphereEntity = ModelEntity(mesh: mesh, materials: [material])
+        sphereEntity.isEnabled = false
+
+        let anchorEntity = AnchorEntity(world: .zero)
+        anchorEntity.addChild(sphereEntity)
+        arView.scene.addAnchor(anchorEntity)
+
+        let coordinator = context.coordinator
+        coordinator.sphereEntity = sphereEntity
+
+        // SceneEvents.Update fires every rendered frame on the main thread.
+        // Updating sphere position here (rather than from the Combine sink) means:
+        //   1. The sphere is never "frozen" between 4 Hz UWB pings — it always reflects
+        //      the latest world-space estimate relative to the smoothly moving camera.
+        //   2. We can apply EMA smoothing to hide step-changes from the solver.
+        coordinator.updateSub = arView.scene.subscribe(to: SceneEvents.Update.self) { [weak estimator, weak coordinator, weak sphereEntity, weak arView] _ in
+            guard let estimator, let coordinator, let sphereEntity else { return }
+
+            // EMA-smooth toward the latest estimate every frame.
+            // α ≈ 0.12 at 60 fps → time constant ≈ 120 ms. Hides solver step-changes
+            // without making the sphere feel sluggish.
+            let alpha: Float = 0.12
+            if let target = estimator.anchorPosition {
+                if let current = coordinator.smoothedPosition {
+                    coordinator.smoothedPosition = current + alpha * (target - current)
+                } else {
+                    coordinator.smoothedPosition = target  // snap on first appearance
+                }
+                sphereEntity.setPosition(coordinator.smoothedPosition!, relativeTo: nil)
+                sphereEntity.isEnabled = true
+            } else {
+                coordinator.smoothedPosition = nil
+                sphereEntity.isEnabled = false
+            }
+
+            // Off-screen directional indicator using the smoothed position
+            guard let arView else { return }
+            guard let position = coordinator.smoothedPosition else {
+                if coordinator.lastAngle != nil {
+                    DispatchQueue.main.async { estimator.offScreenAngle = nil }
+                    coordinator.lastAngle = nil
+                }
+                return
+            }
+
+            let isOffScreen: Bool
+            if let proj = arView.project(position) {
+                isOffScreen = !arView.bounds.contains(proj)
+            } else {
+                isOffScreen = true
+            }
+
+            if isOffScreen {
+                guard let camera = arView.session.currentFrame?.camera else { return }
+                let cameraTransform = camera.transform
+                let localPos4 = simd_mul(simd_inverse(cameraTransform),
+                                        simd_float4(position.x, position.y, position.z, 1.0))
+                let angle = Double(atan2(localPos4.y, localPos4.x))
+                if coordinator.lastAngle == nil || abs(coordinator.lastAngle! - angle) > 0.05 {
+                    coordinator.lastAngle = angle
+                    DispatchQueue.main.async { estimator.offScreenAngle = angle }
+                }
+            } else {
+                if coordinator.lastAngle != nil {
+                    DispatchQueue.main.async { estimator.offScreenAngle = nil }
+                    coordinator.lastAngle = nil
+                }
+            }
+        }
+
+        return arView
+    }
+
+    func updateUIView(_ uiView: ARView, context: Context) {}
+
+    func makeCoordinator() -> Coordinator {
+        Coordinator()
+    }
+
+    class Coordinator {
+        var sphereEntity: ModelEntity?
+        var updateSub: (any Cancellable)?
+        var smoothedPosition: simd_float3? = nil
+        var lastAngle: Double?
+    }
+}

NearbyDemo/Assets.xcassets/AccentColor.colorset/Contents.json

@@ -0,0 +1,11 @@
+{
+  "colors" : [
+    {
+      "idiom" : "universal"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

NearbyDemo/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -0,0 +1,35 @@
+{
+  "images" : [
+    {
+      "idiom" : "universal",
+      "platform" : "ios",
+      "size" : "1024x1024"
+    },
+    {
+      "appearances" : [
+        {
+          "appearance" : "luminosity",
+          "value" : "dark"
+        }
+      ],
+      "idiom" : "universal",
+      "platform" : "ios",
+      "size" : "1024x1024"
+    },
+    {
+      "appearances" : [
+        {
+          "appearance" : "luminosity",
+          "value" : "tinted"
+        }
+      ],
+      "idiom" : "universal",
+      "platform" : "ios",
+      "size" : "1024x1024"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

NearbyDemo/Assets.xcassets/Contents.json

@@ -0,0 +1,6 @@
+{
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

NearbyDemo/ContentView.swift

@@ -0,0 +1,117 @@
+import SwiftUI
+import ARKit
+
+struct ContentView: View {
+    @EnvironmentObject var ble: BLEManager
+    @EnvironmentObject var ni: NIManager
+    @EnvironmentObject var ar: ARManager
+    @EnvironmentObject var estimator: AnchorEstimator
+
+    var body: some View {
+        ZStack {
+            ARViewContainer(arManager: ar, estimator: estimator)
+                .ignoresSafeArea()
+
+            if let angle = estimator.offScreenAngle {
+                Image(systemName: "location.north.fill")
+                    .resizable()
+                    .aspectRatio(contentMode: .fit)
+                    .frame(width: 40, height: 40)
+                    .foregroundColor(.red)
+                    .shadow(radius: 5)
+                    // The image points "Up".
+                    // SwiftUI rotates clockwise. Angle is mathematically CCW.
+                    // If angle = 0 (right), we need it to point Right, so rotate 90 deg clockwise.
+                    // Actually, if image points UP, we rotate by PI/2 - angle.
+                    .rotationEffect(.radians(.pi / 2 - angle))
+                    // Offset moves it towards the edge
+                    .offset(x: cos(angle) * 140, y: -sin(angle) * 140)
+                    .animation(.interactiveSpring(), value: angle)
+            }
+
+            VStack {
+                HUDView(ble: ble, ni: ni, estimator: estimator)
+                    .padding()
+                Spacer()
+                Button("Reset Estimate") {
+                    estimator.reset()
+                }
+                .buttonStyle(.borderedProminent)
+                .tint(.red.opacity(0.8))
+                .padding(.bottom, 40)
+            }
+        }
+    }
+}
+
+// MARK: - HUD overlay
+
+private struct HUDView: View {
+    @ObservedObject var ble: BLEManager
+    @ObservedObject var ni: NIManager
+    @ObservedObject var estimator: AnchorEstimator
+
+    var body: some View {
+        VStack(alignment: .leading, spacing: 6) {
+            HUDRow(label: "BLE", value: bleStateText)
+            HUDRow(label: "NI", value: niStateText)
+            HUDRow(label: "Range", value: rangeText)
+            HUDRow(label: "Measurements", value: "\(estimator.measurementCount)")
+            HUDRow(label: "Residual", value: String(format: "%.3f m", estimator.residualError))
+        }
+        .padding(12)
+        .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 12))
+        .frame(maxWidth: .infinity, alignment: .leading)
+    }
+
+    private var bleStateText: String {
+        switch ble.connectionState {
+        case .disconnected: return "Disconnected"
+        case .scanning:     return "Scanning…"
+        case .connecting:   return "Connecting…"
+        case .connected:    return "Connected"
+        }
+    }
+
+    private var niStateText: String {
+        switch ni.sessionState {
+        case .idle:                 return "Idle"
+        case .waitingForAccessory:  return "Waiting for board…"
+        case .configuring:          return "Configuring…"
+        case .ranging:              return "Ranging"
+        case .error(let msg):       return "Error: \(msg)"
+        }
+    }
+
+    private var rangeText: String {
+        if let r = ni.lastRange {
+            return String(format: "%.2f m", r)
+        }
+        return "—"
+    }
+}
+
+private struct HUDRow: View {
+    let label: String
+    let value: String
+
+    var body: some View {
+        HStack {
+            Text(label)
+                .font(.caption.bold())
+                .foregroundStyle(.secondary)
+                .frame(width: 100, alignment: .leading)
+            Text(value)
+                .font(.caption)
+                .foregroundStyle(.primary)
+        }
+    }
+}
+
+#Preview {
+    ContentView()
+        .environmentObject(BLEManager())
+        .environmentObject(NIManager())
+        .environmentObject(ARManager())
+        .environmentObject(AnchorEstimator())
+}

NearbyDemo/Info.plist

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>UIBackgroundModes</key>
+	<array>
+		<string>bluetooth-central</string>
+		<string>nearby-interaction</string>
+	</array>
+	<key>NSCameraUsageDescription</key>
+	<string>Camera is required for ARKit world tracking to estimate the UWB anchor position in 3D space.</string>
+	<key>NSBluetoothAlwaysUsageDescription</key>
+	<string>Bluetooth is used to communicate with the Qorvo DWM3001CDK UWB anchor and exchange NearbyInteraction session configuration data.</string>
+	<key>NSNearbyInteractionUsageDescription</key>
+	<string>Nearby Interaction is used to measure precise UWB distances to the DWM3001CDK anchor device.</string>
+</dict>
+</plist>

NearbyDemo/Managers/ARManager.swift

@@ -0,0 +1,92 @@
+import ARKit
+import Combine
+import Foundation
+import simd
+
+class ARManager: NSObject, ObservableObject {
+    let session = ARSession()
+
+    @Published var trackingState: ARCamera.TrackingState = .notAvailable
+
+    private(set) var poseBuffer: [TimestampedPose] = []
+    private let bufferCapacity = 120
+    private let bufferLock = NSLock()
+
+    override init() {
+        super.init()
+        session.delegate = self
+    }
+
+    func start() {
+        let config = ARWorldTrackingConfiguration()
+        config.worldAlignment = .gravity
+        config.planeDetection = []
+        config.isAutoFocusEnabled = false
+        session.run(config, options: [.resetTracking, .removeExistingAnchors])
+        print("[AR] Session started")
+    }
+
+    func stop() {
+        session.pause()
+        print("[AR] Session paused")
+    }
+
+    /// Returns the interpolated camera pose at the given system-uptime timestamp,
+    /// or nil if the timestamp is outside the current buffer range.
+    func poseAt(timestamp: TimeInterval) -> simd_float4x4? {
+        bufferLock.lock()
+        let buffer = poseBuffer
+        bufferLock.unlock()
+
+        guard buffer.count >= 2 else { return buffer.first?.transform }
+
+        // Outside range — clamp to edges
+        if timestamp <= buffer.first!.timestamp { return buffer.first?.transform }
+        if timestamp >= buffer.last!.timestamp  { return buffer.last?.transform }
+
+        // Binary search for the bracketing pair
+        var lo = 0, hi = buffer.count - 1
+        while lo + 1 < hi {
+            let mid = (lo + hi) / 2
+            if buffer[mid].timestamp <= timestamp { lo = mid } else { hi = mid }
+        }
+        return PoseInterpolator.interpolate(from: buffer[lo], to: buffer[hi], at: timestamp)
+    }
+}
+
+// MARK: - ARSessionDelegate
+extension ARManager: ARSessionDelegate {
+    func session(_ session: ARSession, didUpdate frame: ARFrame) {
+        let pose = TimestampedPose(transform: frame.camera.transform, timestamp: frame.timestamp)
+
+        bufferLock.lock()
+        poseBuffer.append(pose)
+        if poseBuffer.count > bufferCapacity {
+            poseBuffer.removeFirst()
+        }
+        bufferLock.unlock()
+
+        let state = frame.camera.trackingState
+        DispatchQueue.main.async {
+            self.trackingState = state
+        }
+    }
+
+    func sessionShouldAttemptRelocalization(_ session: ARSession) -> Bool {
+        // NI camera assistance requires this to return false
+        return false
+    }
+
+    func session(_ session: ARSession, didFailWithError error: Error) {
+        print("[AR] Session failed: \(error)")
+    }
+
+    func sessionWasInterrupted(_ session: ARSession) {
+        print("[AR] Session interrupted")
+    }
+
+    func sessionInterruptionEnded(_ session: ARSession) {
+        print("[AR] Session interruption ended — resetting")
+        start()
+    }
+}

NearbyDemo/Managers/AnchorEstimator.swift

@@ -0,0 +1,191 @@
+import Combine
+import Foundation
+import simd
+
+private struct Measurement {
+    let phonePosition: simd_float3
+    let range: Float
+    let cameraForward: simd_float3  // unit vector: -Z column of camera transform at measurement time
+}
+
+class AnchorEstimator: ObservableObject {
+    @Published var anchorPosition: simd_float3? = nil
+    @Published var measurementCount: Int = 0
+    @Published var residualError: Float = 0
+    @Published var offScreenAngle: Double? = nil
+
+    private let windowSize = 50
+    private let outlierThreshold: Float = 0.5
+    private var measurements: [Measurement] = []
+    private var currentEstimate: simd_float3? = nil
+    private var consecutiveOutliers: Int = 0
+    // Skip outlier rejection for the first N measurements so the solver can converge
+    // before the gate starts locking it in place.
+    private var bootstrapCount: Int = 0
+    private let bootstrapThreshold = 15
+
+    // MARK: - Public interface
+
+    /// Feed a UWB range measurement paired with the camera pose at that moment.
+    /// cameraForward is the unit vector pointing in the camera's -Z direction (world frame).
+    func addMeasurement(phonePosition: simd_float3, range: Float,
+                        cameraForward: simd_float3 = simd_float3(0, 0, -1)) {
+        bootstrapCount += 1
+        let isBootstrapping = bootstrapCount <= bootstrapThreshold
+
+        // Outlier rejection against current estimate (disabled during bootstrap so the solver
+        // can converge before the gate starts rejecting corrective measurements).
+        if !isBootstrapping, let est = currentEstimate {
+            let predicted = simd_length(phonePosition - est)
+            if abs(predicted - range) > outlierThreshold {
+                consecutiveOutliers += 1
+                print("[Estimator] Rejected outlier (\(consecutiveOutliers)): predicted=\(predicted) measured=\(range)")
+
+                if consecutiveOutliers >= 10 {
+                    print("[Estimator] 10 consecutive outliers — estimate stuck. Resetting.")
+                    reset()
+                }
+                return
+            }
+        }
+
+        consecutiveOutliers = 0
+        measurements.append(Measurement(phonePosition: phonePosition, range: range, cameraForward: cameraForward))
+        if measurements.count > windowSize {
+            measurements.removeFirst()
+        }
+
+        guard measurements.count >= 4 else { return }
+
+        // Motion spread gate: require the phone to have moved meaningfully within the measurement
+        // window before running the solver. When all positions are nearly identical the system
+        // is underdetermined — the solver output is anywhere on a sphere and can jump arbitrarily.
+        let centroid = measurements.reduce(simd_float3.zero) { $0 + $1.phonePosition } / Float(measurements.count)
+        let rmsSpread = sqrt(measurements.reduce(Float(0)) { acc, m in
+            acc + simd_length_squared(m.phonePosition - centroid)
+        } / Float(measurements.count))
+
+        guard rmsSpread > 0.08 else {
+            // Phone hasn't moved enough — keep previous estimate without a new solve.
+            return
+        }
+
+        currentEstimate = gaussNewton(measurements: measurements, initial: currentEstimate)
+
+        let pos = currentEstimate!
+        let residuals = measurements.map { m -> Float in
+            let d = simd_length(m.phonePosition - pos)
+            return d - m.range
+        }
+        let rmse = sqrt(residuals.map { $0 * $0 }.reduce(0, +) / Float(residuals.count))
+
+        DispatchQueue.main.async {
+            self.anchorPosition = pos
+            self.measurementCount = self.measurements.count
+            self.residualError = rmse
+        }
+    }
+
+    /// Directly set the anchor position from an external source (e.g. NI camera assistance).
+    func setKnownPosition(_ position: simd_float3) {
+        currentEstimate = position
+        DispatchQueue.main.async {
+            self.anchorPosition = position
+        }
+    }
+
+    func reset() {
+        measurements.removeAll()
+        currentEstimate = nil
+        consecutiveOutliers = 0
+        bootstrapCount = 0
+        DispatchQueue.main.async {
+            self.anchorPosition = nil
+            self.measurementCount = 0
+            self.residualError = 0
+        }
+    }
+
+    // MARK: - Gauss-Newton solver
+
+    private func gaussNewton(measurements: [Measurement], initial: simd_float3?) -> simd_float3 {
+        var x: simd_float3
+        if let prior = initial {
+            x = prior
+        } else {
+            // Initialize on the sphere centered at the most recent phone position, at the measured
+            // range, pointing in the camera-forward direction at the time of that measurement.
+            // This satisfies the most recent range constraint approximately and gives the solver
+            // a geometrically valid starting point rather than an arbitrary 1 m offset.
+            let lastM = measurements.last!
+            x = lastM.phonePosition + lastM.range * lastM.cameraForward
+        }
+
+        let maxIterations = 10
+        let convergenceThreshold: Float = 1e-4
+
+        for _ in 0..<maxIterations {
+            var JtJ = simd_float3x3(0)
+            var Jtf = simd_float3.zero
+            var inlierCount = 0
+
+            for m in measurements {
+                let diff = m.phonePosition - x
+                let dist = simd_length(diff)
+                guard dist > 1e-6 else { continue }
+
+                let residual = dist - m.range
+                if abs(residual) > outlierThreshold { continue }
+
+                let J = -(diff / dist)  // 1×3 Jacobian row
+                JtJ.columns.0 += simd_float3(J.x * J.x, J.y * J.x, J.z * J.x)
+                JtJ.columns.1 += simd_float3(J.x * J.y, J.y * J.y, J.z * J.y)
+                JtJ.columns.2 += simd_float3(J.x * J.z, J.y * J.z, J.z * J.z)
+                Jtf += J * residual
+                inlierCount += 1
+            }
+
+            guard inlierCount >= 3 else { break }
+
+            // Tikhonov regularization (Levenberg-Marquardt style): keeps JᵀJ positive-definite
+            // when measurements are collinear, preventing numeric blow-up.
+            let lambda: Float = 1e-3
+            JtJ.columns.0.x += lambda
+            JtJ.columns.1.y += lambda
+            JtJ.columns.2.z += lambda
+
+            guard let JtJinv = inverse3x3(JtJ) else { break }
+            let delta = -(JtJinv * Jtf)
+            x += delta
+
+            if simd_length(delta) < convergenceThreshold { break }
+        }
+
+        return x
+    }
+
+    /// Analytical 3×3 matrix inverse. Returns nil if the matrix is singular.
+    private func inverse3x3(_ m: simd_float3x3) -> simd_float3x3? {
+        let c0 = m.columns.0, c1 = m.columns.1, c2 = m.columns.2
+
+        let r0 = simd_float3(
+            c1.y * c2.z - c1.z * c2.y,
+            c0.z * c2.y - c0.y * c2.z,
+            c0.y * c1.z - c0.z * c1.y
+        )
+        let r1 = simd_float3(
+            c1.z * c2.x - c1.x * c2.z,
+            c0.x * c2.z - c0.z * c2.x,
+            c0.z * c1.x - c0.x * c1.z
+        )
+        let r2 = simd_float3(
+            c1.x * c2.y - c1.y * c2.x,
+            c0.y * c2.x - c0.x * c2.y,
+            c0.x * c1.y - c0.y * c1.x
+        )
+        let det = c0.x * r0.x + c1.x * r0.y + c2.x * r0.z
+        guard abs(det) > 1e-10 else { return nil }
+        let invDet = 1.0 / det
+        return simd_float3x3(columns: (r0 * invDet, r1 * invDet, r2 * invDet))
+    }
+}

NearbyDemo/Managers/BLEManager.swift

@@ -0,0 +1,188 @@
+import CoreBluetooth
+import Combine
+import Foundation
+
+private extension Data {
+    var hexString: String { map { String(format: "%02x", $0) }.joined(separator: " ") }
+}
+
+enum ConnectionState {
+    case disconnected, scanning, connecting, connected
+}
+
+class BLEManager: NSObject, ObservableObject {
+    @Published var connectionState: ConnectionState = .disconnected
+
+    /// Called with raw data when the TX characteristic notifies.
+    var onAccessoryData: ((Data) -> Void)?
+    /// Called when a peripheral successfully connects, passing its UUID.
+    var onConnected: ((UUID) -> Void)?
+
+    /// The identifier of the currently connected peripheral, if any.
+    private(set) var connectedPeripheralIdentifier: UUID?
+
+    private var centralManager: CBCentralManager!
+    private var peripheral: CBPeripheral?
+    private var rxCharacteristic: CBCharacteristic?
+    private var txCharacteristic: CBCharacteristic?
+
+    // Whether to scan all peripherals (fallback) or filter by service UUID.
+    private var broadScan = false
+
+    override init() {
+        super.init()
+        centralManager = CBCentralManager(delegate: self, queue: nil)
+    }
+
+    func startScanning() {
+        guard centralManager.state == .poweredOn else {
+            // Will be triggered again in centralManagerDidUpdateState
+            return
+        }
+        connectionState = .scanning
+        // Try targeted scan first
+        broadScan = false
+        centralManager.scanForPeripherals(withServices: [QorvoBLEUUIDs.niService], options: nil)
+        print("[BLE] Scanning for NI service peripherals…")
+
+        // Fall back to broad scan after 5 seconds if nothing found
+        DispatchQueue.main.asyncAfter(deadline: .now() + 5) { [weak self] in
+            guard let self, self.connectionState == .scanning else { return }
+            print("[BLE] No service-filtered result. Falling back to broad scan.")
+            self.broadScan = true
+            self.centralManager.stopScan()
+            self.centralManager.scanForPeripherals(withServices: nil, options: nil)
+        }
+    }
+
+    func stopScanning() {
+        centralManager.stopScan()
+    }
+
+    func sendToAccessory(_ data: Data) {
+        guard let peripheral, let rx = rxCharacteristic else {
+            print("[BLE] sendToAccessory: no peripheral/rx, dropping \(data.hexString)")
+            return
+        }
+        // Use withoutResponse for small messages only — the iOS BLE stack silently
+        // drops withoutResponse writes that exceed maximumWriteValueLength (often 20
+        // bytes before MTU negotiation). For larger payloads (e.g. 0x0B + config)
+        // use withResponse, which the nRF52 SoftDevice will accept up to 247 bytes.
+        let maxNoRsp = peripheral.maximumWriteValueLength(for: .withoutResponse)
+        let canUseNoRsp = rx.properties.contains(.writeWithoutResponse) && data.count <= maxNoRsp
+        let writeType: CBCharacteristicWriteType = canUseNoRsp ? .withoutResponse : .withResponse
+        print("[BLE] → TX \(data.count) bytes (MTU max=\(maxNoRsp), type=\(writeType == .withResponse ? "rsp" : "no-rsp")): \(data.hexString)")
+        peripheral.writeValue(data, for: rx, type: writeType)
+    }
+
+    func disconnect() {
+        guard let peripheral else { return }
+        centralManager.cancelPeripheralConnection(peripheral)
+    }
+}
+
+// MARK: - CBCentralManagerDelegate
+extension BLEManager: CBCentralManagerDelegate {
+    func centralManagerDidUpdateState(_ central: CBCentralManager) {
+        if central.state == .poweredOn {
+            startScanning()
+        } else {
+            connectionState = .disconnected
+            print("[BLE] Central state: \(central.state.rawValue)")
+        }
+    }
+
+    func centralManager(_ central: CBCentralManager,
+                        didDiscover peripheral: CBPeripheral,
+                        advertisementData: [String: Any],
+                        rssi RSSI: NSNumber) {
+        // When doing a broad scan, match by name prefix
+        if broadScan {
+            let name = peripheral.name ?? ""
+            guard name.hasPrefix("Qorvo") || name.hasPrefix("DWM") || name.contains("UWB") else { return }
+        }
+
+        print("[BLE] Discovered: \(peripheral.name ?? "unknown") \(peripheral.identifier)")
+        centralManager.stopScan()
+        self.peripheral = peripheral
+        connectionState = .connecting
+        centralManager.connect(peripheral, options: nil)
+    }
+
+    func centralManager(_ central: CBCentralManager, didConnect peripheral: CBPeripheral) {
+        print("[BLE] Connected to \(peripheral.name ?? peripheral.identifier.uuidString)")
+        connectionState = .connected
+        connectedPeripheralIdentifier = peripheral.identifier
+        peripheral.delegate = self
+        peripheral.discoverServices([QorvoBLEUUIDs.niService])
+        // onConnected is deferred until TX notifications are confirmed enabled
+    }
+
+    func centralManager(_ central: CBCentralManager, didFailToConnect peripheral: CBPeripheral, error: Error?) {
+        print("[BLE] Failed to connect: \(error?.localizedDescription ?? "unknown")")
+        connectionState = .disconnected
+        self.peripheral = nil
+    }
+
+    func centralManager(_ central: CBCentralManager, didDisconnectPeripheral peripheral: CBPeripheral, error: Error?) {
+        print("[BLE] Disconnected: \(error?.localizedDescription ?? "clean")")
+        connectionState = .disconnected
+        connectedPeripheralIdentifier = nil
+        self.peripheral = nil
+        rxCharacteristic = nil
+        txCharacteristic = nil
+    }
+}
+
+// MARK: - CBPeripheralDelegate
+extension BLEManager: CBPeripheralDelegate {
+    func peripheral(_ peripheral: CBPeripheral, didDiscoverServices error: Error?) {
+        guard error == nil else {
+            print("[BLE] Service discovery error: \(error!)")
+            return
+        }
+        for service in peripheral.services ?? [] {
+            print("[BLE] Discovered service: \(service.uuid)")
+            peripheral.discoverCharacteristics([QorvoBLEUUIDs.rxCharacteristic, QorvoBLEUUIDs.txCharacteristic], for: service)
+        }
+    }
+
+    func peripheral(_ peripheral: CBPeripheral, didDiscoverCharacteristicsFor service: CBService, error: Error?) {
+        guard error == nil else {
+            print("[BLE] Characteristic discovery error: \(error!)")
+            return
+        }
+        for characteristic in service.characteristics ?? [] {
+            print("[BLE] Discovered characteristic: \(characteristic.uuid) props: \(characteristic.properties.rawValue)")
+            if characteristic.uuid == QorvoBLEUUIDs.rxCharacteristic {
+                rxCharacteristic = characteristic
+            } else if characteristic.uuid == QorvoBLEUUIDs.txCharacteristic {
+                txCharacteristic = characteristic
+                peripheral.setNotifyValue(true, for: characteristic)
+            }
+        }
+    }
+
+    func peripheral(_ peripheral: CBPeripheral, didUpdateNotificationStateFor characteristic: CBCharacteristic, error: Error?) {
+        if let error {
+            print("[BLE] Notify enable error on \(characteristic.uuid): \(error)")
+            return
+        }
+        guard characteristic.uuid == QorvoBLEUUIDs.txCharacteristic, characteristic.isNotifying else { return }
+        print("[BLE] TX notifications enabled — ready for NI handshake")
+        // Both characteristics discovered and notifications live: signal ready
+        onConnected?(peripheral.identifier)
+    }
+
+    func peripheral(_ peripheral: CBPeripheral, didUpdateValueFor characteristic: CBCharacteristic, error: Error?) {
+        guard error == nil, let data = characteristic.value else { return }
+        print("[BLE] ← RX \(data.count) bytes: \(data.hexString)")
+        onAccessoryData?(data)
+    }
+
+    func peripheral(_ peripheral: CBPeripheral, didWriteValueFor characteristic: CBCharacteristic, error: Error?) {
+        if let error {
+            print("[BLE] Write error on \(characteristic.uuid): \(error)")
+        }
+    }
+}

NearbyDemo/Managers/NIManager.swift

@@ -0,0 +1,248 @@
+import NearbyInteraction
+import ARKit
+import Combine
+import Foundation
+import CoreBluetooth
+import QuartzCore
+import simd
+
+enum SessionState: Equatable {
+    case idle
+    case waitingForAccessory  // BLE ready, waiting for board to speak first
+    case configuring
+    case ranging
+    case error(String)
+}
+
+// Apple NI Accessory Protocol — confirmed against DW3_QM33_SDK_1.1.1 firmware source
+// (Src/Comm/Src/BLE/niq/ble_niq.c, Libs/niq/Inc/niq.h)
+//
+// Board → iOS  (GATT notify on TX characteristic):
+//   0x01  Accessory Configuration Data — board's UWB config, sent in reply to 0x0A
+//   0x02  UWB Did Start
+//   0x03  UWB Did Stop
+//
+// iOS → Board  (GATT write to RX characteristic):
+//   0x0A  Init — iOS sends this first to request the board's UWB config
+//   0x0B  Configure and Start — iOS sends NI-generated shareable config
+//   0x0C  Stop
+private enum MsgAccessoryToApp: UInt8 {
+    case accessoryConfigData = 0x01
+    case uwbDidStart         = 0x02
+    case uwbDidStop          = 0x03
+}
+
+private enum MsgAppToAccessory: UInt8 {
+    case init_             = 0x0A  // iOS → Board: triggers board to send its UWB config
+    case configureAndStart = 0x0B  // iOS → Board: send NI-generated shareable config
+    case stop              = 0x0C  // iOS → Board: stop ranging
+}
+
+class NIManager: NSObject, ObservableObject {
+    @Published var sessionState: SessionState = .idle
+    @Published var lastRange: Float? = nil
+
+    /// Called with (distance in meters, timestamp) on each range update (fallback when no camera assistance).
+    var onRangeUpdate: ((Float, TimeInterval) -> Void)?
+    /// Called with the accessory's world-space position when camera assistance resolves it.
+    var onWorldPositionUpdate: ((simd_float3) -> Void)?
+    /// Called with outbound BLE data to write to the accessory.
+    var sendToAccessory: ((Data) -> Void)?
+
+    // Peripheral identifier set externally before start() is called.
+    var peripheralIdentifier: UUID?
+    // Set this to the app's ARSession before start() so NI can share it for camera assistance.
+    weak var arSession: ARSession?
+
+    private var niSession: NISession?
+    private var restartWorkItem: DispatchWorkItem?
+
+    /// Called by the app when BLE is connected and TX notifications are live.
+    /// Sends the MessageId_init (0x0A) command to trigger the board to reply
+    /// with its Accessory Configuration Data (0x01 + UWB config payload).
+    func start() {
+        guard sessionState == .idle else { return }
+        sessionState = .waitingForAccessory
+        let initMsg = Data([MsgAppToAccessory.init_.rawValue])
+        sendToAccessory?(initMsg)
+        print("[NI] → Sent Init (0x0A) — waiting for board Accessory Config (0x01)")
+    }
+
+    func handleAccessoryData(_ data: Data) {
+        guard !data.isEmpty else { return }
+        let tag = data[0]
+        let hex = data.map { String(format: "%02x", $0) }.joined(separator: " ")
+        print("[NI] ← rx tag=0x\(String(tag, radix: 16, uppercase: false)) len=\(data.count): \(hex)")
+
+        switch tag {
+        case MsgAccessoryToApp.accessoryConfigData.rawValue:
+            // Board replied to our 0x0A with 0x01 + AccessoryConfigurationData payload
+            let payload = Data(data.dropFirst())
+            guard !payload.isEmpty else {
+                print("[NI] 0x01 received but payload is empty — ignoring")
+                return
+            }
+            print("[NI] Board sent Accessory Config (0x01) — \(payload.count) bytes, starting NI session")
+            startNISession(with: payload)
+
+        case MsgAccessoryToApp.uwbDidStart.rawValue:
+            print("[NI] Board confirmed UWB Did Start (0x02)")
+            DispatchQueue.main.async { self.sessionState = .ranging }
+
+        case MsgAccessoryToApp.uwbDidStop.rawValue:
+            print("[NI] Board confirmed UWB Did Stop (0x03)")
+            DispatchQueue.main.async {
+                self.sessionState = .idle
+                self.lastRange = nil
+            }
+
+        default:
+            print("[NI] Unexpected tag 0x\(String(tag, radix: 16, uppercase: false)) (\(data.count) bytes) — ignoring")
+        }
+    }
+
+    func stop() {
+        restartWorkItem?.cancel()
+        sendToAccessory?(Data([MsgAppToAccessory.stop.rawValue]))
+        niSession?.invalidate()
+        niSession = nil
+        DispatchQueue.main.async {
+            self.sessionState = .idle
+            self.lastRange = nil
+        }
+    }
+
+    // MARK: - Private
+
+    private func startNISession(with accessoryData: Data) {
+        guard sessionState == .waitingForAccessory || sessionState == .idle else {
+            print("[NI] startNISession skipped — already in state \(sessionState)")
+            return
+        }
+        sessionState = .configuring
+        print("[NI] Creating NINearbyAccessoryConfiguration from \(accessoryData.count) bytes")
+
+        // Use initWithData: (not initWithAccessoryData:bluetoothPeerIdentifier:).
+        // The peer-identifier variant requires a bonded/paired Bluetooth device;
+        // the DWM3001CDK is connected but not bonded, so that path silently prevents
+        // didGenerateShareableConfigurationData from ever firing.
+        // We handle all BLE transport manually, so the simple init is correct.
+        let configuration: NINearbyAccessoryConfiguration
+        do {
+            configuration = try NINearbyAccessoryConfiguration(data: accessoryData)
+        } catch {
+            let hexDump = accessoryData.map { String(format: "%02x", $0) }.joined(separator: " ")
+            print("[NI] Configuration parse error: \(error)\n     data: \(hexDump)")
+            DispatchQueue.main.async { self.sessionState = .waitingForAccessory }
+            return
+        }
+
+        // Enable camera assistance when the device supports it (iOS 16+, U1/U2 chip).
+        // This fuses ARKit visual-inertial odometry with UWB ranging inside Apple's framework,
+        // giving us a world-space position via worldTransform(for:) without running our own solver.
+        if #available(iOS 16.0, *), NISession.deviceCapabilities.supportsCameraAssistance {
+            configuration.isCameraAssistanceEnabled = true
+            print("[NI] Camera assistance enabled")
+        }
+
+        let session = NISession()
+        session.delegate = self
+        niSession = session
+
+        // Share our existing ARSession so NI doesn't spin up a second one.
+        // Must be called before run(_:). The session must already be running with a compatible config.
+        if #available(iOS 16.0, *), configuration.isCameraAssistanceEnabled, let arSession {
+            session.setARSession(arSession)
+            print("[NI] Shared ARSession with NISession")
+        }
+
+        session.run(configuration)
+        print("[NI] NISession running")
+    }
+
+    /// Send the Configure-and-Start message with both the canonical (0x0B) and
+    /// alternate (0x05) bytes so the board accepts it regardless of firmware version.
+    private func sendConfigureAndStart(_ shareableData: Data) {
+        // Try 0x0B first (Apple spec canonical byte)
+        var payload = Data([MsgAppToAccessory.configureAndStart.rawValue])
+        payload.append(shareableData)
+        sendToAccessory?(payload)
+        print("[NI] → Sent Configure and Start (0x0B) \(payload.count) bytes")
+    }
+}
+
+// MARK: - NISessionDelegate
+extension NIManager: NISessionDelegate {
+    func session(_ session: NISession, didUpdate nearbyObjects: [NINearbyObject]) {
+        guard let object = nearbyObjects.first, let distance = object.distance else { return }
+        let timestamp = CACurrentMediaTime()
+        DispatchQueue.main.async {
+            self.lastRange = distance
+            self.sessionState = .ranging
+        }
+
+        // Prefer camera-assisted world position: Apple's framework fuses UWB + ARKit VIO internally.
+        // worldTransform(for:) returns nil until camera assistance has converged.
+        if #available(iOS 16.0, *),
+           let worldTransform = session.worldTransform(for: object) {
+            let worldPos = simd_float3(worldTransform.columns.3.x,
+                                       worldTransform.columns.3.y,
+                                       worldTransform.columns.3.z)
+            onWorldPositionUpdate?(worldPos)
+        } else {
+            // Fall back to manual range+pose fusion via AnchorEstimator
+            onRangeUpdate?(distance, timestamp)
+        }
+    }
+
+    func session(_ session: NISession, didRemove nearbyObjects: [NINearbyObject], reason: NINearbyObject.RemovalReason) {
+        print("[NI] Objects removed reason=\(reason.rawValue) — restarting NI handshake in 1 s")
+        DispatchQueue.main.async {
+            self.sessionState = .idle
+            self.lastRange = nil
+        }
+        // BLE is still connected — re-send 0x0A to restart the NI handshake
+        let work = DispatchWorkItem { [weak self] in
+            guard let self else { return }
+            DispatchQueue.main.async { self.sessionState = .idle }
+            self.start()
+        }
+        restartWorkItem = work
+        DispatchQueue.main.asyncAfter(deadline: .now() + 1, execute: work)
+    }
+
+    func session(_ session: NISession, didUpdateAlgorithmConvergence convergence: NIAlgorithmConvergence, for object: NINearbyObject?) {
+        print("[NI] Algorithm convergence: \(convergence.status)")
+    }
+
+    func session(_ session: NISession, didGenerateShareableConfigurationData shareableConfigurationData: Data, for object: NINearbyObject) {
+        print("[NI] didGenerateShareableConfigurationData — \(shareableConfigurationData.count) bytes")
+        sendConfigureAndStart(shareableConfigurationData)
+    }
+
+    func sessionWasSuspended(_ session: NISession) {
+        print("[NI] Session suspended")
+        DispatchQueue.main.async { self.sessionState = .idle }
+    }
+
+    func sessionSuspensionEnded(_ session: NISession) {
+        print("[NI] Suspension ended — rerunning configuration")
+        guard let config = session.configuration else { return }
+        session.run(config)
+    }
+
+    func session(_ session: NISession, didInvalidateWith error: Error) {
+        print("[NI] Session invalidated: \(error)")
+        DispatchQueue.main.async {
+            self.sessionState = .error(error.localizedDescription)
+            self.niSession = nil
+        }
+        let work = DispatchWorkItem { [weak self] in
+            guard let self else { return }
+            DispatchQueue.main.async { self.sessionState = .idle }
+            self.start()
+        }
+        restartWorkItem = work
+        DispatchQueue.main.asyncAfter(deadline: .now() + 1, execute: work)
+    }
+}

NearbyDemo/Models/QorvoBLEUUIDs.swift

@@ -0,0 +1,9 @@
+import CoreBluetooth
+
+enum QorvoBLEUUIDs {
+    static let niService        = CBUUID(string: "2E938FD0-6A61-11ED-A1EB-0242AC120002")
+    // App writes accessory configuration / commands to this characteristic
+    static let rxCharacteristic = CBUUID(string: "2E93998A-6A61-11ED-A1EB-0242AC120002")
+    // Accessory notifies the app with ranging data / responses on this characteristic
+    static let txCharacteristic = CBUUID(string: "2E939AF2-6A61-11ED-A1EB-0242AC120002")
+}

NearbyDemo/Models/TimestampedPose.swift

@@ -0,0 +1,7 @@
+import simd
+import Foundation
+
+struct TimestampedPose {
+    let transform: simd_float4x4
+    let timestamp: TimeInterval
+}

NearbyDemo/Models/TimestampedRange.swift

@@ -0,0 +1,6 @@
+import Foundation
+
+struct TimestampedRange {
+    let range: Float
+    let timestamp: TimeInterval
+}

NearbyDemo/NearbyDemoApp.swift

@@ -0,0 +1,67 @@
+import SwiftUI
+import ARKit
+import simd
+
+@main
+struct NearbyDemoApp: App {
+    @StateObject private var ble = BLEManager()
+    @StateObject private var ni  = NIManager()
+    @StateObject private var ar  = ARManager()
+    @StateObject private var estimator = AnchorEstimator()
+
+    var body: some Scene {
+        WindowGroup {
+            ContentView()
+                .environmentObject(ble)
+                .environmentObject(ni)
+                .environmentObject(ar)
+                .environmentObject(estimator)
+                .onAppear {
+                    wireManagers()
+                    ar.start()
+                    ble.startScanning()
+                }
+        }
+    }
+
+    private func wireManagers() {
+        // BLE → NI: forward raw accessory bytes into the NI state machine
+        ble.onAccessoryData = { [weak ni] data in
+            ni?.handleAccessoryData(data)
+        }
+
+        // NI → BLE: send outbound messages to the accessory
+        ni.sendToAccessory = { [weak ble] data in
+            ble?.sendToAccessory(data)
+        }
+
+        // Share the ARSession with NI for camera assistance. Must be set before BLE connects
+        // and triggers startNISession, which calls setARSession(_:) before session.run(_:).
+        ni.arSession = ar.session
+
+        // BLE connected → start NI session
+        ble.onConnected = { [weak ni] peripheralID in
+            ni?.peripheralIdentifier = peripheralID
+            ni?.start()
+        }
+
+        // NI camera-assisted world position → set directly on estimator, bypassing Gauss-Newton.
+        // Apple's framework fuses UWB + ARKit VIO internally; this is more accurate than our solver.
+        ni.onWorldPositionUpdate = { [weak estimator] position in
+            estimator?.setKnownPosition(position)
+        }
+
+        // NI range-only updates → fuse with AR pose → feed Gauss-Newton estimator.
+        // This runs when camera assistance hasn't converged yet or isn't supported.
+        ni.onRangeUpdate = { [weak ar, weak estimator] range, timestamp in
+            guard let ar, let estimator else { return }
+            guard let pose = ar.poseAt(timestamp: timestamp) else { return }
+            let position = simd_float3(pose.columns.3.x, pose.columns.3.y, pose.columns.3.z)
+            // Camera forward in world space: -Z column of the camera transform
+            let cameraForward = simd_normalize(simd_float3(-pose.columns.2.x,
+                                                           -pose.columns.2.y,
+                                                           -pose.columns.2.z))
+            estimator.addMeasurement(phonePosition: position, range: range, cameraForward: cameraForward)
+        }
+    }
+}

NearbyDemo/Utilities/PoseInterpolator.swift

@@ -0,0 +1,27 @@
+import simd
+import Foundation
+
+struct PoseInterpolator {
+    /// Interpolates between two timestamped poses at the given timestamp.
+    /// Uses linear interpolation for translation and spherical linear interpolation for rotation.
+    static func interpolate(from a: TimestampedPose, to b: TimestampedPose, at timestamp: TimeInterval) -> simd_float4x4 {
+        let t = Float((timestamp - a.timestamp) / (b.timestamp - a.timestamp))
+        let clamped = max(0, min(1, t))
+        return lerp(from: a.transform, to: b.transform, t: clamped)
+    }
+
+    /// Interpolates between two simd_float4x4 transforms with a given t in [0, 1].
+    static func lerp(from a: simd_float4x4, to b: simd_float4x4, t: Float) -> simd_float4x4 {
+        let tA = simd_float3(a.columns.3.x, a.columns.3.y, a.columns.3.z)
+        let tB = simd_float3(b.columns.3.x, b.columns.3.y, b.columns.3.z)
+        let qA = simd_quatf(a)
+        let qB = simd_quatf(b)
+
+        let tInterp = tA + (tB - tA) * t
+        let qInterp = simd_slerp(qA, qB, t)
+
+        var result = simd_float4x4(qInterp)
+        result.columns.3 = simd_float4(tInterp.x, tInterp.y, tInterp.z, 1.0)
+        return result
+    }
+}