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. )
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
}
}
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);
}