我将尝试使用Unity卡尔曼滤波器。但我发现了一个问题。
应用卡尔曼滤波器后，位置应用得很好。然而，旋转并没有得到很好的应用。当物体的旋转（x或y或z）从正变为负或从负变为正时，物体就会翻转（可能是360°？/我附上参考视频。）
我能弄清楚如何解决这个问题吗？或者Unity中有完整的卡尔曼滤波器源码吗？
由于我使用Unity，旋转使用四元数。但我的卡尔曼滤波器似乎使用欧拉。我将其更改为向量 4，但无法修复它。
** 1.控制器代码
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.更新
using UnityEngine;
using System.Collections;
namespace Kalman {
public interface IKalmanWrapper : System.IDisposable
{
Vector3 Update (Vector3 current);
}
}
**卡尔曼滤波器代码
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;
}
}
}
**矩阵卡尔曼包装器
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
}
}
这是因为欧拉角位于超过 [0,360) 的模空间（可能是糟糕的术语）。
我对卡尔曼滤波器一无所知，但这是一个可能的部分解决方案。也许它会引导你找到答案
使用 2 个滤波器来估计局部
transform.uptransform.forwardQuaternion.LookRotationvoid 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);
}
如何实现旋转和位置？