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