void Start()
{
rb = GetComponent <Rigidbody>();
fireAudio = GetComponent <AudioSource>();
mKalmanX = new KalmanFilter(Input.acceleration.x);
mKalmanY = new KalmanFilter(Input.acceleration.y);
}
public void KalmanFilter()
{
mousePoints = new List <PointF>();
kalmanPoints = new List <PointF>();
kal = new KalmanFilter(4, 2, 0);
syntheticData = new SyntheticData();
Matrix <float> state = new Matrix <float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f
});
/*
* kal.CorrectedState = state;
* kal.TransitionMatrix = syntheticData.transitionMatrix;
* kal.MeasurementNoiseCovariance = syntheticData.measurementNoise;
* kal.ProcessNoiseCovariance = syntheticData.processNoise;
* kal.ErrorCovariancePost = syntheticData.errorCovariancePost;
* kal.MeasurementMatrix = syntheticData.measurementMatrix;
*/
state.Mat.CopyTo(kal.StatePre);
syntheticData.transitionMatrix.Mat.CopyTo(kal.TransitionMatrix); // kal.TransitionMatrix = syntheticData.transitionMatrix;
syntheticData.measurementNoise.Mat.CopyTo(kal.MeasurementNoiseCov); // kal.MeasurementNoiseCovariance = syntheticData.measurementNoise;
syntheticData.processNoise.Mat.CopyTo(kal.ProcessNoiseCov); // kal.ProcessNoiseCovariance = syntheticData.processNoise;
syntheticData.errorCovariancePost.Mat.CopyTo(kal.ErrorCovPost); // kal.ErrorCovariancePost = syntheticData.errorCovariancePost;
syntheticData.measurementMatrix.Mat.CopyTo(kal.MeasurementMatrix); // kal.MeasurementMatrix = syntheticData.measurementMatrix;
}
void Awake()
{
skeletonManager = FindObjectOfType(typeof(RUISSkeletonManager)) as RUISSkeletonManager;
if (skeletonController != null)
{
playerId = skeletonController.playerId;
}
else
{
Debug.LogError("The public variable 'Skeleton Controller' is not assigned! Using skeleton "
+ "#0 as the torso position source.");
}
if (gameObject.transform.parent != null)
{
if (gameObject.transform.parent.GetComponentInChildren <RUISKinectAndMecanimCombiner>())
{
kinectAndMecanimCombinerExists = true;
}
}
capsuleCollider = GetComponent <CapsuleCollider>();
defaultColliderHeight = capsuleCollider.height;
defaultColliderPosition = transform.localPosition;
positionKalman = new KalmanFilter();
positionKalman.initialize(3, 3);
positionKalman.skipIdenticalMeasurements = true;
}
public KalmanTracking3D(Track t, double ErrorX, double ErrorY, double ErrorZ, double ErrorSx, double ErrorSy,
double NoiseX, double NoiseY, double NoiseSx, double NoiseSy)
{
//Assignment to private variables
m_Track = t;
m_ErrorX = ErrorX;
m_ErrorY = ErrorY;
m_ErrorZ = ErrorZ;
m_ErrorSx = ErrorSx;
m_ErrorSy = ErrorSy;
m_NoiseX = NoiseX;
m_NoiseY = NoiseY;
m_NoiseSx = NoiseSx;
m_NoiseSy = NoiseSy;
kf = new KalmanFilter();
m_fp = new double[6, 6];
m_bp = new double[6, 6];
m_mc = new double[5, 5] {
{ ErrorX, 0, 0, 0, 0 }, { 0, ErrorSx, 0, 0, 0 }, { 0, 0, ErrorY, 0, 0 }, { 0, 0, 0, ErrorSy, 0 }, { 0, 0, 0, 0, ErrorZ }
};
m_nc = new double[6, 6] {
{ NoiseX, 0, 0, 0, 0, 0 }, { 0, NoiseSx, 0, 0, 0, 0 }, { 0, 0, NoiseY, 0, 0, 0 }, { 0, 0, 0, NoiseSy, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
};
m_st = new double[5, 6] {
{ 1, 0, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 }, { 0, 0, 1, 0, 0, 0 }, { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, 0 }
};
m_fc = new double[6, 6] {
{ 0.01, 0, 0, 0, 0, 0 }, { 0, 0.01, 0, 0, 0, 0 }, { 0, 0, 0.01, 0, 0, 0 }, { 0, 0, 0, 0.01, 0, 0 }, { 0, 0, 0, 0, 0.01, 0 }, { 0, 0, 0, 0, 0, 0.01 }
};
SetKalmanFilter();
}
public SimpleKalmanTracking(double ErrorX, double ErrorY, double ErrorSx, double ErrorSy,
double NoiseX, double NoiseY, double NoiseSx, double NoiseSy, double ProjectionZ)
{
//Assignment to private variables
m_ProjectionZ = ProjectionZ;
m_ErrorX = ErrorX;
m_ErrorY = ErrorY;
m_ErrorSx = ErrorSx;
m_ErrorSy = ErrorSy;
m_NoiseX = NoiseX;
m_NoiseY = NoiseY;
m_NoiseSx = NoiseSx;
m_NoiseSy = NoiseSy;
kf = new KalmanFilter();
m_fp = new double[4, 4];
m_bp = new double[4, 4];
m_mc = new double[4, 4] {
{ ErrorX, 0, 0, 0 }, { 0, ErrorSx, 0, 0 }, { 0, 0, ErrorY, 0 }, { 0, 0, 0, ErrorSy }
};
m_nc = new double[4, 4] {
{ NoiseX, 0, 0, 0 }, { 0, NoiseSx, 0, 0 }, { 0, 0, NoiseY, 0 }, { 0, 0, 0, NoiseSy }
};
m_st = new double[4, 4] {
{ 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 }
};
m_fc = new double[4, 4] {
{ 0.01, 0, 0, 0 }, { 0, 0.01, 0, 0 }, { 0, 0, 0.01, 0 }, { 0, 0, 0, 0.01 }
};
}
public void TestKalman()
{
double[] measurements = new double[] { 5, 6, 7, 9, 10 };
double[] motion = new double[] { 1, 1, 2, 1, 1 };
double mu = 0;
double sigma = 1000;
double measureSigma = 4.0;
double motionSig = 2.0;
double[] expectedUpdateMu = new double[] { 4.98, 5.992, 6.996, 8.998, 9.999 };
double[] expectedPredictMu = new double[] { 5.98, 6.992, 8.996, 9.998, 10.999 };
double[] expectedUpdateSigma = new double[] { 3.984, 2.397, 2.095, 2.023, 2.005 };
double[] expectedPredictSigma = new double[] { 5.984, 4.397, 4.095, 4.023, 4.006 };
KalmanFilter k = new KalmanFilter(new Gaussian(mu, sigma));
for (int i = 0; i < measurements.Length; i++)
{
k.Update(measurements[i], measureSigma);
Assert.AreEqual(expectedUpdateMu[i], k.Mu, 0.001);
Assert.AreEqual(expectedUpdateSigma[i], k.Sigma, 0.001);
k.Predict(motion[i], motionSig);
Assert.AreEqual(expectedPredictMu[i], k.Mu, 0.001);
Assert.AreEqual(expectedPredictSigma[i], k.Sigma, 0.001);
}
}
private void initializeKalman()
{
float accelNoise = (float)numProcessNoise.Value;
float measurementNoise = (float)numMeasurementNoise.Value;
var measurementDimension = 2; //just coordinates
var initialState = process.GetNoisyState(accelNoise); //assuming we measured process params (noise)
var initialStateError = ModelState.GetProcessNoise(accelNoise);
kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(accelNoise);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, measurementNoise).ElementwisePower(2); //assuming we measured process params (noise) - ^2 => variance
kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{ 1, 0, 0, 0 }, //picks X
{ 0, 0, 1, 0 } //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix(ConstantVelocityProcess.TimeInterval);
}
private void initializeKalman()
{
float accelNoise = (float)numProcessNoise.Value;
float measurementNoise = (float)numMeasurementNoise.Value;
var measurementDimension = 2; //just coordinates
var initialState = process.GetNoisyState(accelNoise); //assuming we measured process params (noise)
var initialStateError = ModelState.GetProcessNoise(accelNoise);
kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(accelNoise);
kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, measurementNoise).ElementwisePower(2); //assuming we measured process params (noise) - ^2 => variance
kalman.MeasurementMatrix = new double[,] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{1, 0, 0, 0}, //picks X
{0, 0, 1, 0} //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix(ConstantVelocityProcess.TimeInterval);
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState {
Position = startPoint, Velocity = new PointF(0.5f, -2f)
};
var initialStateError = ModelState.GetProcessNoise(0);
kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(2);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, 1);
kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{ 1, 0, 0, 0 }, //picks X
{ 0, 0, 1, 0 } //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
void Awake()
{
skeletonManager = FindObjectOfType(typeof(RUISSkeletonManager)) as RUISSkeletonManager;
if(skeletonController != null)
playerId = skeletonController.playerId;
else
Debug.LogError( "The public variable 'Skeleton Controller' is not assigned! Using skeleton "
+ "ID 0 as the pivot source.");
if(gameObject.transform.parent != null)
{
if(gameObject.transform.parent.GetComponentInChildren<RUISKinectAndMecanimCombiner>())
kinectAndMecanimCombinerExists = true;
}
capsuleCollider = GetComponent<CapsuleCollider>();
if(capsuleCollider == null)
Debug.LogError("GameObject " + gameObject.name + " must have a CapsuleCollider!");
defaultColliderHeight = capsuleCollider.height;
defaultColliderPosition = transform.localPosition;
positionKalman = new KalmanFilter();
positionKalman.initialize(3,3);
positionKalman.skipIdenticalMeasurements = true;
}
private void SetKalmanFilter()
{
int j, n;
n = m_Track.Length;
int ng;
kf = new KalmanFilter();
double[] mrows = new double[4];
for (j = 0; j < n; j++)
{
mrows[0] = m_Track[j].Info.Intercept.X;
mrows[1] = m_Track[j].Info.Slope.X;
mrows[2] = m_Track[j].Info.Intercept.Y;
mrows[3] = m_Track[j].Info.Slope.Y;
if (j == n - 1)
{
ng = 1;
}
else
{
ng = Math.Abs(m_Track[j + 1].LayerOwner.SheetId - m_Track[j].LayerOwner.SheetId);
}
m_fp = new double[4, 4] {
{ 1, -ng * m_ProjectionZ, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, -ng * m_ProjectionZ }, { 0, 0, 0, 1 }
};
m_bp = new double[4, 4] {
{ 1, ng *m_ProjectionZ, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, ng *m_ProjectionZ }, { 0, 0, 0, 1 }
};
FilterStep fs = new FilterStep(j, mrows, mrows, m_fp, m_bp, m_mc, m_nc, m_st, m_fc);
kf.AddStep(fs);
}
}
private void Awake()
{
SerialController = GameObject.Find("SerialController");
MessageListener = GameObject.Find("MessageListener");
KalmanFilterScript = gameObject.GetComponent <KalmanFilter>();
ReadCSVScript = gameObject.GetComponent <ReadCSV>();
KalmanFilterScript.init(Application.dataPath + "/" + outputFileName);
}
public PointServiceSource(
TimeSpan pointTtl,
IPointService pointGeneratingService)
{
this.pointTtl = pointTtl;
this.pointGeneratingService = pointGeneratingService;
this.kalmanFilter = new KalmanFilter();
}
private void ApplyKalmanFilters(ref double x, ref double y)
{
if (KalmanX == null)
{
KalmanX = new KalmanFilter(x, 0.5, 5, 1000);
KalmanY = new KalmanFilter(y, 0.5, 5, 1000);
}
x = KalmanX.Update(x);
y = KalmanY.Update(y);
}
public void Awake()
{
// velocityBuffer = new List<Vector3>();
if(GetComponent<Rigidbody>())
oldCollisionMode = GetComponent<Rigidbody>().collisionDetectionMode;
positionKalman = new KalmanFilter();
positionKalman.initialize(3,3);
positionKalman.skipIdenticalMeasurements = true;
}
public GazeTrackerSource(
TimeSpan pointTtl,
int gazeTrackerUdpPort,
Regex gazeTrackerRegex)
{
this.pointTtl = pointTtl;
this.gazeTrackerUdpPort = gazeTrackerUdpPort;
this.gazeTrackerRegex = gazeTrackerRegex;
this.kalmanFilter = new KalmanFilter();
}
private void processMeas(KalmanFilter f,
List <DMatrixRMaj> meas)
{
DMatrixRMaj R = CommonOps_DDRM.identity(measDOF);
foreach (DMatrixRMaj z in meas)
{
f.predict();
f.update(z, R);
}
}
private void compareFilters(KalmanFilter a, KalmanFilter b)
{
DMatrixRMaj testX = b.getState();
DMatrixRMaj testP = b.getCovariance();
DMatrixRMaj X = a.getState();
DMatrixRMaj P = a.getCovariance();
EjmlUnitTests.assertEquals(testX, X, UtilEjml.TEST_F64);
EjmlUnitTests.assertEquals(testP, P, UtilEjml.TEST_F64);
}
public FilterQuaternion(float processNoise = 1, float measurementNoise = 1, int period = 5, int queueCount = 15)
{
_period = period;
_quaternionQueue = new FixedSizedQueue <GyroQuaternion>(queueCount);
_kalmanFilterX = new KalmanFilter(processNoise, measurementNoise);
_kalmanFilterY = new KalmanFilter(processNoise, measurementNoise);
_kalmanFilterZ = new KalmanFilter(processNoise, measurementNoise);
_kalmanFilterW = new KalmanFilter(processNoise, measurementNoise);
}
public void Update(TimeSpan ts, Matrix <double> z)
{
//dFilterSharp.UpdateQ(AdaptiveQ, ts, z, H, AdaptiveR.Value);
KalmanFilter.UpdateState(z, H, R);
//dFilterSharp.UpdateR(AdaptiveR, ts, z, H);
//return dFilterSharp;
}
public PointServiceSource(
TimeSpan pointTtl,
IPointService pointGeneratingService)
{
this.pointTtl = pointTtl;
this.pointGeneratingService = pointGeneratingService;
this.kalmanFilterX = new KalmanFilter(Settings.Default.KalmanFilterInitialValue, Settings.Default.KalmanFilterConfidenceOfInitialValue,
Settings.Default.KalmanFilterProcessNoise, Settings.Default.KalmanFilterMeasurementNoise);
this.kalmanFilterY = new KalmanFilter(Settings.Default.KalmanFilterInitialValue, Settings.Default.KalmanFilterConfidenceOfInitialValue,
Settings.Default.KalmanFilterProcessNoise, Settings.Default.KalmanFilterMeasurementNoise);
}
public void Awake()
{
// velocityBuffer = new List<Vector3>();
if (GetComponent <Rigidbody>())
{
oldCollisionMode = GetComponent <Rigidbody>().collisionDetectionMode;
}
positionKalman = new KalmanFilter();
positionKalman.initialize(3, 3);
positionKalman.skipIdenticalMeasurements = true;
}
public GazeTrackerSource(
TimeSpan pointTtl,
int gazeTrackerUdpPort,
Regex gazeTrackerRegex)
{
this.pointTtl = pointTtl;
this.gazeTrackerUdpPort = gazeTrackerUdpPort;
this.gazeTrackerRegex = gazeTrackerRegex;
this.kalmanFilterX = new KalmanFilter(Settings.Default.KalmanFilterInitialValue, Settings.Default.KalmanFilterConfidenceOfInitialValue,
Settings.Default.KalmanFilterProcessNoise, Settings.Default.KalmanFilterMeasurementNoise);
this.kalmanFilterY = new KalmanFilter(Settings.Default.KalmanFilterInitialValue, Settings.Default.KalmanFilterConfidenceOfInitialValue,
Settings.Default.KalmanFilterProcessNoise, Settings.Default.KalmanFilterMeasurementNoise);
}
private void OnEnable()
{
var ds4 = GetFirstDs4(Player);
if (ds4 == null)
{
ds4Found = false;
return;
}
ds4Found = true;
isWorking = false;
m_KalmanFilter = new KalmanFilter(0.21f, 0.01f, 4);
InvokeRepeating(nameof(ReadData), 0.5f, 1.0f / frequency);
}
public void StatePre()
{
using var kalman = new KalmanFilter();
using var propValue = kalman.StatePre;
Assert.NotNull(propValue);
Assert.True(propValue.Empty());
using var mat = new Mat(2, 2, MatType.CV_8U, new byte[, ]
{
{ 1, 1 },
{ 0, 1 }
});
kalman.StatePre = mat;
IsDataEqual(mat, kalman.StatePre);
}
void Awake()
{
if (skeletonManager == null)
{
skeletonManager = FindObjectOfType(typeof(RUISSkeletonManager)) as RUISSkeletonManager;
}
followMoveController = false;
jointInitialRotations = new Dictionary <Transform, Quaternion>();
jointInitialDistances = new Dictionary <KeyValuePair <Transform, Transform>, float>();
positionKalman = new KalmanFilter();
positionKalman.initialize(3, 3);
}
public void MeasurementMatrix()
{
using var kalman = new KalmanFilter();
using var propValue = kalman.MeasurementMatrix;
Assert.NotNull(propValue);
Assert.True(propValue.Empty());
using var mat = new Mat(2, 2, MatType.CV_8U, new byte[, ]
{
{ 1, 1 },
{ 0, 1 }
});
kalman.MeasurementMatrix = mat;
IsDataEqual(mat, kalman.MeasurementMatrix);
}
public void ProcessNoiseCov()
{
using var kalman = new KalmanFilter();
using var propValue = kalman.ProcessNoiseCov;
Assert.NotNull(propValue);
Assert.True(propValue.Empty());
using var mat = new Mat(2, 2, MatType.CV_8U, new byte[, ]
{
{ 1, 1 },
{ 0, 1 }
});
kalman.ProcessNoiseCov = mat;
IsDataEqual(mat, kalman.ProcessNoiseCov);
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.2f, -1.5f)};
var initialStateError = ModelState.GetProcessNoise(10);
kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(1);
kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, 10000.0);
kalman.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix();
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
private void SetKalmanFilter()
{
int j, n;
n = m_Track.Length;
double projz;
kf = new KalmanFilter();
double[] srows = new double[6];
double[] mrows = new double[5];
for (j = 0; j < n; j++)
{
srows[0] = m_Track[j].Info.Intercept.X;
srows[1] = m_Track[j].Info.Slope.X;
srows[2] = m_Track[j].Info.Intercept.Y;
srows[3] = m_Track[j].Info.Slope.Y;
srows[4] = m_Track[j].Info.Intercept.Z;
srows[5] = 1;
mrows[0] = srows[0];
mrows[1] = srows[1];
mrows[2] = srows[2];
mrows[3] = srows[3];
mrows[4] = srows[4];
if (j == n - 1)
{
projz = Math.Abs(m_Track[j].Info.Intercept.Z - m_Track[j - 1].Info.Intercept.Z);
}
else
{
projz = Math.Abs(m_Track[j + 1].Info.Intercept.Z - m_Track[j].Info.Intercept.Z);
}
m_fp = new double[6, 6] {
{ 1, -projz, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 }, { 0, 0, 1, -projz, 0, 0 }, { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, -projz }, { 0, 0, 0, 0, 0, 1 }
};
m_bp = new double[6, 6] {
{ 1, projz, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 }, { 0, 0, 1, projz, 0, 0 }, { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, projz }, { 0, 0, 0, 0, 0, 1 }
};
FilterStep fs = new FilterStep(j, mrows, srows, m_fp, m_bp, m_mc, m_nc, m_st, m_fc);
kf.AddStep(fs);
}
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState {
Position = startPoint, Velocity = new PointF(0.2f, -1.5f)
};
var initialStateError = ModelState.GetProcessNoise(10);
kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(1);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, 10000.0);
kalman.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix();
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
public static IObservable <AngularVelocity3> Filter(
this IObservable <AngularVelocity3> source,
double estimatedMeasurementNoiseCovarianceX, double estimatedProcessNoiseCovarianceX, double controlWeightX, double initialEstimateX, double initialErrorCovarianceX,
double estimatedMeasurementNoiseCovarianceY, double estimatedProcessNoiseCovarianceY, double controlWeightY, double initialEstimateY, double initialErrorCovarianceY,
double estimatedMeasurementNoiseCovarianceZ, double estimatedProcessNoiseCovarianceZ, double controlWeightZ, double initialEstimateZ, double initialErrorCovarianceZ)
{
var filterX = new KalmanFilter(estimatedMeasurementNoiseCovarianceX, estimatedProcessNoiseCovarianceX, controlWeightX, initialEstimateX, initialErrorCovarianceX);
var filterY = new KalmanFilter(estimatedMeasurementNoiseCovarianceY, estimatedProcessNoiseCovarianceY, controlWeightY, initialEstimateY, initialErrorCovarianceY);
var filterZ = new KalmanFilter(estimatedMeasurementNoiseCovarianceZ, estimatedProcessNoiseCovarianceZ, controlWeightZ, initialEstimateZ, initialErrorCovarianceZ);
return(source
.Select(angularVelocity =>
{
return new AngularVelocity3(
filterX.Filter(angularVelocity.X),
filterY.Filter(angularVelocity.Y),
filterZ.Filter(angularVelocity.Z));
}));
}
/// <summary>
/// kalman 初期化ルーチン
/// </summary>
/// <param name="elem">読み出した要素</param>
public void kalman_init()
{
KalmanFilter kalman = kv_kalman;
// 初期化(kalman)
kalman_id = 0;
Cv2.SetIdentity(kalman.MeasurementMatrix, new Scalar(1.0));
Cv2.SetIdentity(kalman.ProcessNoiseCov, new Scalar(1e-4));
Cv2.SetIdentity(kalman.MeasurementNoiseCov, new Scalar(0.001));
Cv2.SetIdentity(kalman.ErrorCovPost, new Scalar(1.0));
measurement = measurement.SetTo(0.0);// measurement.Zeor(); MatType.CV_64FC1);
// 等速直線運動モデル(kalman)
/* unsafe
* {
* kalman.DynamMatr[0] = 1.0f; kalman.DynamMatr[1] = 0.0f; kalman.DynamMatr[2] = 1.0f; kalman.DynamMatr[3] = 0.0f;
* kalman.DynamMatr[4] = 0.0f; kalman.DynamMatr[5] = 1.0f; kalman.DynamMatr[6] = 0.0f; kalman.DynamMatr[7] = 1.0f;
* kalman.DynamMatr[8] = 0.0f; kalman.DynamMatr[9] = 0.0f; kalman.DynamMatr[10] = 1.0f; kalman.DynamMatr[11] = 0.0f;
* kalman.DynamMatr[12] = 0.0f; kalman.DynamMatr[13] = 0.0f; kalman.DynamMatr[14] = 0.0f; kalman.DynamMatr[15] = 1.0f;
* }*/
kalman.TransitionMatrix.Set(0, 0, 1.0f);
kalman.TransitionMatrix.Set(0, 1, 0.0f);
kalman.TransitionMatrix.Set(0, 2, 1.0f);
kalman.TransitionMatrix.Set(0, 3, 0.0f);
kalman.TransitionMatrix.Set(1, 0, 0.0f);
kalman.TransitionMatrix.Set(1, 1, 1.0f);
kalman.TransitionMatrix.Set(1, 2, 0.0f);
kalman.TransitionMatrix.Set(1, 3, 1.0f);
kalman.TransitionMatrix.Set(2, 0, 0.0f);
kalman.TransitionMatrix.Set(2, 1, 0.0f);
kalman.TransitionMatrix.Set(2, 2, 1.0f);
kalman.TransitionMatrix.Set(2, 3, 0.0f);
kalman.TransitionMatrix.Set(3, 0, 0.0f);
kalman.TransitionMatrix.Set(3, 1, 0.0f);
kalman.TransitionMatrix.Set(3, 2, 0.0f);
kalman.TransitionMatrix.Set(3, 3, 1.0f);
}
private void SetupKalmanFilter(Skeleton skel)
{
kal = new KalmanFilter(4, 2, 0);
syntheticData = new JumpData();
// Initial state of the skeleton, positioned at 0.
Matrix <float> state = new Matrix <float>(new float[]
{
0.0f, 0.0f,
0.0f, 0.0f
});
// Transfer references into the kalman-filter
state.Mat.CopyTo(kal.StatePost); // == correctedstate
syntheticData.transition.Mat.CopyTo(kal.TransitionMatrix);
syntheticData.measurementNoise.Mat.CopyTo(kal.MeasurementNoiseCov);
syntheticData.processNoise.Mat.CopyTo(kal.ProcessNoiseCov);
syntheticData.errorCovariancePost.Mat.CopyTo(kal.ErrorCovPost);
syntheticData.measurement.Mat.CopyTo(kal.MeasurementMatrix);
}
void Awake ()
{
skeletonManager = FindObjectOfType(typeof(RUISSkeletonManager)) as RUISSkeletonManager;
if(gameObject.transform.parent != null)
{
if(gameObject.transform.parent.GetComponentInChildren<RUISKinectAndMecanimCombiner>())
kinectAndMecanimCombinerExists = true;
}
capsuleCollider = GetComponent<CapsuleCollider>();
if(capsuleCollider == null)
Debug.LogError("GameObject " + gameObject.name + " must have a CapsuleCollider!");
defaultColliderHeight = capsuleCollider.height;
defaultColliderPosition = transform.localPosition;
positionKalman = new KalmanFilter();
positionKalman.initialize(3,3);
positionKalman.skipIdenticalMeasurements = true;
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.5f, -2f)};
var initialStateError = ModelState.GetProcessNoise(0);
kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(2);
kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, 1);
kalman.MeasurementMatrix = new double[,] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{1, 0, 0, 0}, //picks X
{0, 0, 1, 0} //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
void Awake()
{
if (skeletonManager == null)
{
skeletonManager = FindObjectOfType(typeof(RUISSkeletonManager)) as RUISSkeletonManager;
}
followMoveController = false;
jointInitialRotations = new Dictionary<Transform, Quaternion>();
jointInitialDistances = new Dictionary<KeyValuePair<Transform, Transform>, float>();
positionKalman = new KalmanFilter();
positionKalman.initialize(3,3);
}
// Use this for initialization
void Start()
{
outputTransform = transform;
filterPos = new KalmanFilter();
filterPos.initialize(3,3);
}
public void Awake()
{
filterDrift = new KalmanFilter();
filterDrift.initialize(2,2);
}
void Awake()
{
inputManager = FindObjectOfType(typeof(RUISInputManager)) as RUISInputManager;
if(inputManager)
{
if(switchToAvailableKinect)
{
if( bodyTrackingDevice == bodyTrackingDeviceType.Kinect1
&& !inputManager.enableKinect && inputManager.enableKinect2)
{
bodyTrackingDevice = bodyTrackingDeviceType.Kinect2;
}
else if( bodyTrackingDevice == bodyTrackingDeviceType.Kinect2
&& !inputManager.enableKinect2 && inputManager.enableKinect)
{
bodyTrackingDevice = bodyTrackingDeviceType.Kinect1;
}
}
}
coordinateSystem = FindObjectOfType(typeof(RUISCoordinateSystem)) as RUISCoordinateSystem;
if(bodyTrackingDevice == bodyTrackingDeviceType.Kinect1) bodyTrackingDeviceID = RUISSkeletonManager.kinect1SensorID;
if(bodyTrackingDevice == bodyTrackingDeviceType.Kinect2) bodyTrackingDeviceID = RUISSkeletonManager.kinect2SensorID;
if(bodyTrackingDevice == bodyTrackingDeviceType.GenericMotionTracker) bodyTrackingDeviceID = RUISSkeletonManager.customSensorID;
followOculusController = false;
followMoveController = false;
trackedDeviceYawRotation = Quaternion.identity;
jointInitialRotations = new Dictionary<Transform, Quaternion>();
jointInitialDistances = new Dictionary<KeyValuePair<Transform, Transform>, float>();
positionKalman = new KalmanFilter();
positionKalman.initialize(3,3);
for(int i=0; i<fourJointsKalman.Length; ++i)
{
fourJointsKalman[i] = new KalmanFilter();
fourJointsKalman[i].initialize(3,3);
fourJointPositions[i] = Vector3.zero;
}
}
// End of Yaw Drift Corrector members
void Awake()
{
localPosition = Vector3.zero;
localRotation = Quaternion.identity;
rawRotation = Quaternion.identity;
filterPos = new KalmanFilter();
filterPos.initialize(3,3);
filterPos.skipIdenticalMeasurements = true;
// filterRot = new KalmanFilter();
// filterRot.initialize(4,4);
// Mobile Razer Hydra base filtering
hydraBaseFilterPos = new KalmanFilter();
hydraBaseFilterPos.initialize(3,3);
hydraBaseFilterPos.skipIdenticalMeasurements = true;
// hydraBaseFilterRot = new KalmanFilter();
// hydraBaseFilterRot.initialize(4,4);
filterRot.skipIdenticalMeasurements = true;
// Yaw Drift Corrector invocations in Awake()
filterDrift = new KalmanFilter();
filterDrift.initialize(2,2);
transform.localPosition = defaultPosition;
eyeCenterPosition = defaultPosition;
measuredHeadPosition = defaultPosition;
hydraBasePosition = new Vector3(0, 0, 0);
hydraBaseRotation = Quaternion.identity;
oculusCamController = gameObject.GetComponentInChildren(typeof(OVRCameraController)) as OVRCameraController;
if(oculusCamController)
{
useOculusRiftRotation = true;
}
else
{
useOculusRiftRotation = false;
}
//oculusCamController = FindObjectOfType(typeof(OVRCameraController)) as OVRCameraController;
//if(headRotationInput == HeadRotationSource.OculusRift && !oculusCamController)
// Debug.LogError("OVRCameraController script is missing from this scene!");
filterPosition = false;
}
/// <summary>
/// Initialize the rotation Kalman filter
/// </summary>
public KalmanFilteredRotation ()
{
rotationState = Quaternion.identity;
filterRot = new KalmanFilter();
filterRot.initialize(4,4);
}
void Awake()
{
coordinateSystem = MonoBehaviour.FindObjectOfType(typeof(RUISCoordinateSystem)) as RUISCoordinateSystem;
localPosition = Vector3.zero;
localRotation = Quaternion.identity;
rawRotation = Quaternion.identity;
filterPos = new KalmanFilter();
filterPos.initialize(3,3);
filterPos.skipIdenticalMeasurements = true;
// filterRot = new KalmanFilter();
// filterRot.initialize(4,4);
// Mobile Razer Hydra base filtering
hydraBaseFilterPos = new KalmanFilter();
hydraBaseFilterPos.initialize(3,3);
hydraBaseFilterPos.skipIdenticalMeasurements = true;
// hydraBaseFilterRot = new KalmanFilter();
// hydraBaseFilterRot.initialize(4,4);
filterRot.skipIdenticalMeasurements = true;
// Yaw Drift Corrector invocations in Awake()
filterDrift = new KalmanFilter();
filterDrift.initialize(2,2);
transform.localPosition = defaultPosition;
eyeCenterPosition = defaultPosition;
measuredHeadPosition = defaultPosition;
hydraBasePosition = new Vector3(0, 0, 0);
hydraBaseRotation = Quaternion.identity;
ovrCameraRig = GetComponentInChildren<OVRCameraRig>();
if(ovrCameraRig != null && OVRManager.display != null && OVRManager.display.isPresent)
{
useOculusRiftRotation = true;
}
else
{
useOculusRiftRotation = false;
}
// Enforce rotation settings if rotation source is set to be same as position source
if (!pickRotationSource)
{
switch (headPositionInput)
{
case HeadPositionSource.Kinect1:
{
headRotationInput = HeadRotationSource.Kinect1;
rotationPlayerID = positionPlayerID;
rotationJoint = positionJoint;
break;
}
case HeadPositionSource.Kinect2:
{
headRotationInput = HeadRotationSource.Kinect2;
rotationPlayerID = positionPlayerID;
rotationJoint = positionJoint;
break;
}
case HeadPositionSource.PSMove:
{
headRotationInput = HeadRotationSource.PSMove;
rotationPSMoveID = positionPSMoveID;
break;
}
case HeadPositionSource.RazerHydra:
{
headRotationInput = HeadRotationSource.RazerHydra;
rotationRazerID = positionRazerID;
break;
}
case HeadPositionSource.InputTransform:
{
headRotationInput = HeadRotationSource.InputTransform;
rotationInput = positionInput;
break;
}
case HeadPositionSource.None:
{
headRotationInput = HeadRotationSource.None;
break;
}
}
}
filterPosition = false;
}
