How to figure out Kalman Filter Rotation flip problem

Question

I will try to use the Unity Kalman filter. But I Caught a problem.

After applying the Kalman filter, position is applied well. However, rotation is not applied well. When Object's rotation(x or y or z) change from positive to negative or from negative to positive, the object is flipped(maybe 360º ? / I attach a video of reference. )

Reference GIF

Can I figure out how to solve this problem? Or is there a complete Kalman filter source in Unity?

Since I use Unity, rotation uses quaternions. But my Kalman filter seems to use Euler. I changed this to vector 4, but it was not possible to fix it.

** 1. Controller Code

using UnityEngine;
using Kalman;

public class Test : MonoBehaviour {

[SerializeField]
Camera cam;

[SerializeField]
Transform nonFilter; //Input Object (not Filter)

[SerializeField]
Transform filterdCube; //Object to be filtered

IKalmanWrapper kalman;
IKalmanWrapper kalman2;
Vector3 nonFilterRot;
Vector3 nonFilterPos;

void Awake ()
{
    kalman = new MatrixKalmanWrapper ();
    kalman2 = new MatrixKalmanWrapper();
    //kalman = new SimpleKalmanWrapper ();
}

void Start ()
{
    cam = Camera.main;
}

// Update is called once per frame
void Update ()
{
    nonFilterRot = nonFilter.transform.rotation.eulerAngles; //make euler
    nonFilterPos = nonFilter.transform.position;    

    filterdCube.transform.position = kalman.Update(nonFilterPos);
    filterdCube.transform.rotation = Quaternion.Euler(kalman2.Update(nonFilterRot)); //Go to Kalman Filter
}
}

** 2.Update

using UnityEngine;
using System.Collections;


namespace Kalman {
    public interface IKalmanWrapper : System.IDisposable
    {
        Vector3 Update (Vector3 current);
    }
}

**Kalman Filter Code

namespace Kalman
{
    public sealed class KalmanFilter
{
    //System matrices
    public Matrix X0 { get; private set; }  // predicted state
    public Matrix P0 { get; private set; }  // predicted covariance

    public Matrix F { get; private set; }   // factor of real value to previous real value
    public Matrix B { get; private set; }   // the control-input model which is applied to the control vector uk;
    public Matrix U { get; private set; }   // the control-input model which is applied to the control vector uk;
    public Matrix Q { get; private set; }   // measurement noise
    public Matrix H { get; private set; }   // factor of measured value to real value
    public Matrix R { get; private set; }   // environment noise

    public Matrix State { get; private set; } 
    public Matrix Covariance { get; private set; }

    public KalmanFilter(Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r)
    {
        F = f;
        B = b;
        U = u;
        Q = q;
        H = h;
        R = r;
    }

    public void SetState(Matrix state, Matrix covariance)
    {
        // Set initial state
        State = state;
        Covariance = covariance;
    }

    public void Correct (Matrix z)
    {
        // Predict
        //X0 = F * State +(B * U);
        X0 = F * State;
        P0 = F * Covariance * F.Transpose () + Q;

        // Correct
        //var k = P0 * H.Transpose() * (H * P0 * H.Transpose() + R).Inverse(); // kalman gain
        var k = P0 * H.Transpose () * (H * P0 * H.Transpose () + R).Invert (); // kalman gain
        State = X0 + (k * (z - (H * X0)));
        //Covariance = (Matrix.Identity (P0.RowCount) - k * H) * P0;
        Covariance = (Matrix.IdentityMatrix (P0.rows) - k * H) * P0;
    }

}

}

**MatrixKalmanWrapper

using UnityEngine;

namespace Kalman {

/// <summary>
/// Matrix kalman wrapper.
/// </summary>
public class MatrixKalmanWrapper : IKalmanWrapper
{
    private KalmanFilter kX;
    private KalmanFilter kY;
    private KalmanFilter kZ;

    public MatrixKalmanWrapper ()
    {
        /*
        X0 : predicted state
        P0 : predicted covariance

        F : factor of real value to previous real value
        B : the control-input model which is applied to the control vector uk;
        U : the control-input model which is applied to the control vector uk;
        Q : measurement noise
        H : factor of measured value to real value
        R : environment noise
        */
        var f = new Matrix (new[,] {{1.0, 1}, {0, 1.0}});
        var b = new Matrix (new[,] {{0.0}, {0}});
        var u = new Matrix (new[,] {{0.0}, {0}});
        var r = Matrix.CreateVector (10);
        //var q = new Matrix(new[,] { { 0.01, 0.4 }, { 0.1, 0.02 } });
        //var h = new Matrix(new[,] { { 1.0, 0 } });
        var q = new Matrix (new[,] {{0.001, 0.001 }, { 0.001, 0.001 } });
        var h = new Matrix (new[,] {{ 1.0  , 0}});

        kX = makeKalmanFilter (f, b, u, q, h, r);
        kY = makeKalmanFilter (f, b, u, q, h, r);
        kZ = makeKalmanFilter (f, b, u, q, h, r);
    }

    public Vector3 Update(Vector3 current)
    {
        kX.Correct(new Matrix(new double[,] { { current.x } }));
        kY.Correct(new Matrix(new double[,] { { current.y } }));
        kZ.Correct(new Matrix(new double[,] { { current.z } }));

        // rashod
        // kX.State [1,0];
        // kY.State [1,0];
        // kZ.State [1,0];

        Vector3 filtered = new Vector3(
            (float)kX.State[0, 0],
            (float)kY.State[0, 0],
            (float)kZ.State[0, 0]
        );
        return filtered;
    }
    public void Dispose ()
    {

    }

    #region Privates
    KalmanFilter makeKalmanFilter (Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r)
    {
        var filter = new KalmanFilter (
            f.Duplicate (),
            b.Duplicate (),
            u.Duplicate (),
            q.Duplicate (),
            h.Duplicate (),
            r.Duplicate ()
        );
        // set initial value
        filter.SetState (
            Matrix.CreateVector (500, 0), 
            new Matrix (new [,] {{10.0, 0}, {0, 5.0}})
        );
        return filter;
    }
    #endregion



}

}

Answer 1

This is due to eulerangles being in a modulo space (bad terminology probably) over [0,360).

I don't know anything about Kalman Filters, but here's a possible partial solution. Maybe it will guide you to an answer

Use 2 filters to estimate the local transform.up and the transform.forward direction and then get the rotation from the estimates with Quaternion.LookRotation

void Awake ()
{
    kalman = new MatrixKalmanWrapper ();
    kalman2 = new MatrixKalmanWrapper();
    kalman3 = new MatrixKalmanWrapper();
}

void Start ()
{
    cam = Camera.main;
}

// Update is called once per frame
void Update ()
{
    nonFilterForward = nonFilter.transform.forward;
    nonFilterUp = nonFilter.transform.up;
    nonFilterPos = nonFilter.transform.position;    

    filterdCube.transform.position = kalman.Update(nonFilterPos);

    Vector3 filteredForward = kalman2.Update(nonFilterForward );
    Vector3 filteredUp = kalman3.Update(nonFilterUp);
    filterdCube.transform.rotation = Quaternion.LookRotation(filteredForward, filteredUp);
}