xref: /packages/modules/Uwb/service/java/com/android/server/uwb/correction/primers/BackAzimuthPrimer.java

/*
 * Copyright (C) 2023 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package com.android.server.uwb.correction.primers;

import static com.android.server.uwb.correction.math.MathHelper.F_HALF_PI;
import static com.android.server.uwb.correction.math.MathHelper.F_PI;
import static com.android.server.uwb.correction.math.MathHelper.normalizeRadians;

import static java.lang.Math.abs;
import static java.lang.Math.max;
import static java.lang.Math.min;
import static java.lang.Math.signum;
import static java.lang.Math.toDegrees;

import android.os.Build;
import android.util.Log;

import androidx.annotation.NonNull;
import androidx.annotation.Nullable;

import com.android.server.uwb.correction.math.MathHelper;
import com.android.server.uwb.correction.math.Pose;
import com.android.server.uwb.correction.math.SphericalVector;
import com.android.server.uwb.correction.math.SphericalVector.Sparse;
import com.android.server.uwb.correction.pose.IPoseSource;

import java.util.ArrayDeque;
import java.util.Queue;

/**
 * Tracks the correlation between azimuth and pose yaw to determine if the controlee has gone
 * to the other side of the device. This mirrors the azimuth value if:
 * - The prediction crosses the ±90° threshold.
 * - The covariance between pose yaw and azimuth exceeds a certain threshold.
 */
public class BackAzimuthPrimer implements IPrimer {
    static final String TAG = "BackAzimuthPrimer";
    private static final boolean sDebug;

    static {
        sDebug = Build.TYPE != null && Build.TYPE.equals("userdebug");
    }

    // Keep sample time measurements reasonable to prevent div/0 or overflows.
    private static final long MIN_TIME_MS = 5; // Shortest allowed time between ranging samples
    private static final long MAX_TIME_MS = 5000; // Longest allowed time between ranging samples

    private final boolean mMaskRawAzimuthWhenBackfacing;
    private final float mDiscrepancyCoefficient;
    private final int mWindowSize;
    private final float mStdDev;
    private boolean mMirrored = false;
    private float mLastAzimuthPrediction;
    private final float mNormalThresholdRadPerSec;
    private final float mMirrorThresholdRadPerSec;
    private final Queue<Float> mScoreHistory;
    private final Queue<Float> mDiscrepancyHistory = new ArrayDeque<>();

    // This initial value causes the first sample to have the least effect.
    private long mLastSampleTimeMs = Long.MIN_VALUE;
    private Pose mLastPose;
    private SphericalVector mLastInput;


    /**
     * Creates a new instance of the BackAzimuthPrimer class.
     *
     * @param normalThresholdRadPerSec How many radians per second of correlated rotation are
     * necessary to force a non-mirrored azimuth.
     * @param mirrorThresholdRadPerSec How many radians per second of correlated rotation are
     * necessary to force a mirrored azimuth.
     * @param windowSize The size of the moving window filter for determining correlation.
     * @param maskRawAzimuthWhenBackfacing If true, readings from the back will be replaced with
     * predictions. If false, azimuth readings will be mirrored front-to-back.
     * @param stdDev Controls the width of the curve used to judge if the readings are acting like
     * unmirrored or mirrored readings.
     * @param discrepancyCoefficient The coefficient of how much the typical forward prediction
     * error (rads per sample) should count against the forward score (rads per second). For
     * example, a value of 0.5 will cause the score to be 5 degrees lower if the typical front
     * prediction error is 10.
     */
    public BackAzimuthPrimer(float normalThresholdRadPerSec,
            float mirrorThresholdRadPerSec,
            int windowSize,
            boolean maskRawAzimuthWhenBackfacing,
            float stdDev,
            float discrepancyCoefficient
    ) {
        mNormalThresholdRadPerSec = normalThresholdRadPerSec;
        mMirrorThresholdRadPerSec = mirrorThresholdRadPerSec;
        mMaskRawAzimuthWhenBackfacing = maskRawAzimuthWhenBackfacing;
        mDiscrepancyCoefficient = discrepancyCoefficient;
        mScoreHistory = new ArrayDeque<>();
        mWindowSize = windowSize;
        mStdDev = stdDev;
    }

    /**
     * Uses pose information to disambiguate the input azimuth.
     *
     * @param input     The original UWB reading.
     * @param prediction The previous filtered UWB result adjusted by the pose change since then.
     * @param poseSource A pose source that may indicate phone orientation.
     * @param timeMs When the input occurred, in ms since boot.
     * @return A replacement value for the UWB vector that has been corrected for the situation.
     */
    @SuppressWarnings("ConstantConditions") /* Unboxing longs in mCaptureTimes */
    @Override
    public SphericalVector.Sparse prime(
            @NonNull SphericalVector.Sparse input,
            @Nullable SphericalVector prediction,
            @Nullable IPoseSource poseSource,
            long timeMs) {
        if (!input.hasAzimuth || poseSource == null || prediction == null) {
            // Can't perform correction if there is no azimuth data, no pose information,
            // or no prediction.
            return input;
        }

        long timeDeltaMs = min(MAX_TIME_MS, max(MIN_TIME_MS, timeMs - mLastSampleTimeMs));
        float timeScale = (float) MathHelper.MS_PER_SEC / timeDeltaMs;
        mLastSampleTimeMs = timeMs;

        // Mirror if the pose simply rotated the azimuth past the 90-degree mark.
        if (abs(prediction.azimuth) > F_HALF_PI
                && abs(mLastAzimuthPrediction) <= F_HALF_PI) {
            // Prediction went from forward to backward-facing.
            mMirrored = true;
            flipScoreHistory();
        } else if (abs(prediction.azimuth) <= F_HALF_PI
                && abs(mLastAzimuthPrediction) > F_HALF_PI) {
            // Prediction went from backward to forward-facing.
            mMirrored = false;
            flipScoreHistory();
        }
        mLastAzimuthPrediction = prediction.azimuth;
        // Because the prediction is influenced by mirroring itself and can have a significant
        //  delay due to the filter, we will not be using the prediction to guess front/back.

        // Get coordinates for normal and mirrored azimuth versions of the input.
        // input.vector may be >90deg due to previous primers, so front/back is forced here.
        SphericalVector normalInput = forceAzimuth(input.vector, false);
        SphericalVector mirrorInput = forceAzimuth(input.vector, true);

        Pose newPose = poseSource.getPose();
        if (mLastPose == null || newPose == null || mLastPose == newPose || mLastInput == null) {
            // Can't do anything without pose deltas and input history.
            mLastPose = newPose;
            mLastInput = normalInput;
            return input;
        }

        // Pose transform for theorizing how the previous reading might have changed.
        // Note that we're using a full pose transform instead of just azimuth changes, as
        // the phone may have rolled or performed other movements that aren't just azimuth.
        Pose deltaPose = Pose.compose(newPose.inverted(), mLastPose);

        // Theorize, based on the old location, what the new location should be for mirrored and
        // unmirrored inputs.
        SphericalVector normalTheory = transformSpherical(mLastInput, deltaPose);
        SphericalVector mirrorTheory = transformSpherical(mirrorAzimuth(mLastInput), deltaPose);

        // Compute how many radians of pose change have affected the azimuth. More movement means
        // more certainty can be applied to the score.
        float azimuthDeltaFromPoseRad =
                normalizeRadians(abs(normalTheory.azimuth - mLastInput.azimuth));

        // Judge how well the front and back predictions did.
        float normalDifference = abs(normalizeRadians(normalTheory.azimuth - normalInput.azimuth));
        float mirrorDifference = abs(normalizeRadians(mirrorTheory.azimuth - mirrorInput.azimuth));
        // Note that one of these predictions will be perfect if the input itself is a prediction,
        // which FovPrimer might do. Carrying this detail in SphericalVector.Sparse may provide an
        // opportunity to ignore scoring when the input is predicted.
        float normalAccuracy = bell(normalDifference, mStdDev);
        float mirrorAccuracy = bell(mirrorDifference, mStdDev);

        // Score by the user's pose-induced azimuth change.
        float scoreRadPerSec = (normalAccuracy - mirrorAccuracy) // score per sample
                * azimuthDeltaFromPoseRad // convert to score radians per sample
                * timeScale; // convert to score radians per second

        // Bias the score toward the back based on UWB noise.
        float scoreRadPerSecBiased = biasScore(scoreRadPerSec, normalDifference, mirrorDifference);

        mLastInput = normalInput;
        mLastPose = newPose;

        mScoreHistory.offer(scoreRadPerSecBiased);

        double typScore = 0;
        if (mScoreHistory.size() > mWindowSize) {
            mScoreHistory.poll();
            // Get the median score.
            typScore = mScoreHistory.stream().mapToDouble(Float::doubleValue).sorted()
                    .skip(mWindowSize / 2).limit(1 + (mWindowSize % 2)).average().getAsDouble();

            // Finally, the mirroring decision.
            if (typScore > mNormalThresholdRadPerSec) {
                mMirrored = false;
            } else if (typScore < -mMirrorThresholdRadPerSec) {
                mMirrored = true;
            }
        }

        if (sDebug) {
            Log.d(TAG,
                    String.format(
                        "time %4d, pose % 6.1f, nd % 6.1f (%3d%%), md % 6.1f (%3d%%), "
                            + "rawSco % 5.1f, sco % 5.1f, aggSco % 5.1f, %s",
                        timeDeltaMs,
                        toDegrees(azimuthDeltaFromPoseRad),
                        toDegrees(normalDifference), (int) (normalAccuracy * 100),
                        toDegrees(mirrorDifference), (int) (mirrorAccuracy * 100),
                        toDegrees(scoreRadPerSec),
                        toDegrees(scoreRadPerSecBiased),
                        toDegrees(typScore),
                        mMirrored ? "mirr" : "norm"
                ));
        }

        SphericalVector result = input.vector;

        if (mMirrored && mMaskRawAzimuthWhenBackfacing) {
            // Replace angles with prediction. The mMaskRawAzimuthWhenBackfacing setting will be set
            // when through-device readings are poor or not predictably mirrored.
            result = SphericalVector.fromRadians(
                prediction.azimuth,
                prediction.elevation,
                input.vector.distance);
        }

        result = forceAzimuth(result, mMirrored);

        return new Sparse(
            result,
            true,
            input.hasElevation,
            input.hasDistance);
    }

    /** Flips the score history, for when azimuth goes from front to behind or vice-versa. */
    private void flipScoreHistory() {
        for (int x = 0; x < mScoreHistory.size(); x++) {
            Float sh = mScoreHistory.poll();
            mScoreHistory.offer(sh == null ? null : -sh);
        }
    }

    /**
     * Biases a score toward the back based on the accuracy of front-facing predictions.
     * @param scoreRadPerSec The score to bias.
     * @param normalDifference The difference between the front-based prediction and front-based
     * reading.
     * @param mirrorDifference The difference between the back-based prediction and back-based
     * reading.
     * @return A new, adjusted version of the input score.
     */
    private float biasScore(float scoreRadPerSec, float normalDifference, float mirrorDifference) {
        // Discrepancy measures how much the forward-facing values are off. They will be WAY off
        // if the UWB signal is noisy, and slightly off if the signal is in the back. Rear azimuths
        // are usually both.
        mDiscrepancyHistory.offer(normalDifference);

        if (mDiscrepancyHistory.size() > mWindowSize) {
            mDiscrepancyHistory.poll();
            float avgDiscrepancyRad =
                    (float) mDiscrepancyHistory.stream().mapToDouble(Float::doubleValue).average()
                            .getAsDouble();

            // Average discrepancy is multiplied by the configurable coefficient to bias the
            //  score toward the back.
            return scoreRadPerSec - avgDiscrepancyRad * mDiscrepancyCoefficient;
        }
        return scoreRadPerSec;
    }

    /**
     * Applies a pose delta (a transform) to a spherical coordinate.
     * @param input The spherical vector to transform.
     * @param deltaPose The pose object representing how to transform the input.
     * @return A new SphericalVector representing the input transformed by the delta pose.
     */
    private SphericalVector transformSpherical(SphericalVector input, Pose deltaPose) {
        return SphericalVector.fromCartesian(deltaPose.transformPoint(input.toCartesian()));
    }

    /**
     * Mirrors the azimuth front-to-back or back-to-front.
     *
     * @param vector The SphericalVector to mirror.
     * @return A mirrored version of the SphericalVector.
     */
    @NonNull
    private SphericalVector mirrorAzimuth(SphericalVector vector) {
        return SphericalVector.fromRadians(
            signum(vector.azimuth) * (F_PI - abs(vector.azimuth)),
            vector.elevation,
            vector.distance);
    }

    /**
     * Forces the azimuth to be front or back, mirroring it as necessary.
     *
     * @param vector The SphericalVector to force to a direction.
     * @param back If true, forces the SphericalVector's azimuth to be back, otherwise forward.
     * @return A version of the SphericalVector that is facing the specified direction.
     */
    @NonNull
    private SphericalVector forceAzimuth(SphericalVector vector, boolean back) {
        if (back == abs(vector.azimuth) < F_HALF_PI) {
            return mirrorAzimuth(vector);
        }
        return vector;
    }

    /**
     * Plots x on a bell curve with a magnitude of 1. This is a gaussian curve(φ) multiplied by
     * 1/φ(0) so that bell(0, n) = 1.
     *
     * @param x The x value of the normal curve.
     * @param stdDev The standard deviation to use for the initial normal curve.
     * @return A value along a gaussian curve scaled by 1/φ(0).
     */
    private float bell(float x, float stdDev) {
        float variance = stdDev * stdDev;
        return (float) Math.exp(-(x * x / (2 * variance)));
    }
}