/*************************************************************************************************
* Public functions & standard Unity callbacks *
*************************************************************************************************/
// Use this for initialization
void Start()
{
if (p != null && p.id == 1)
{
kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
1, 1);
}
else
{
kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
0.1, 0.1);
}
posHistory = new Vector2[60];
posHistoryIterator = 0;
stateTimer = Time.time;
IAState = (p != null && p.id == 0) ? SWIAState.RANDOM : SWIAState.DODGE_ATTACKS;
actionNumber = 0;
//#if UNITY_EDITOR
//#else
target.SetActive(false);
noisedPosition.SetActive(false);
imprecision.SetActive(false);
//#endif
}
public MCvPoint3D32f Kalman4Joints(float x, float y, float z, Kalman kalman, SyntheticData syntheticData)
{
//Kalman headPC1
Emgu.CV.Matrix <float> MatrixGet = new Emgu.CV.Matrix <float>(6, 1);
MatrixGet[0, 0] = x;
MatrixGet[1, 0] = y;
MatrixGet[2, 0] = z;
kalman = new Kalman(6, 3, 0);
Emgu.CV.Matrix <float> state = MatrixGet;
kalman.CorrectedState = state;
kalman.TransitionMatrix = syntheticData.transitionMatrix;
kalman.MeasurementNoiseCovariance = syntheticData.measurementNoise;
kalman.ProcessNoiseCovariance = syntheticData.processNoise;
kalman.ErrorCovariancePost = syntheticData.errorCovariancePost;
kalman.MeasurementMatrix = syntheticData.measurementMatrix;
Matrix <float> prediction = new Matrix <float>(3, 1);
prediction = kalman.Predict();
MCvPoint3D32f predictPointheadPC1 = new MCvPoint3D32f(prediction[0, 0], prediction[1, 0], prediction[2, 0]);
MCvPoint3D32f measurePointheadPC1 = new MCvPoint3D32f(syntheticData.GetMeasurement()[0, 0],
syntheticData.GetMeasurement()[1, 0], syntheticData.GetMeasurement()[2, 0]);
Matrix <float> estimated = kalman.Correct(syntheticData.GetMeasurement());
MCvPoint3D32f estimatedPoint = new MCvPoint3D32f(estimated[0, 0], estimated[1, 0], estimated[2, 0]);
syntheticData.GoToNextState();
return(estimatedPoint);
}
void Start()
{
// Since output rotation is flipped(rotated 180), we have to rotate the model.
// Calculate the model Rotate in advance to save computation
iR = Quaternion.AngleAxis(180, Vector3.forward) * this.transform.localRotation;
this.transform.localPosition = new Vector3(0, 0, -30);
// Position adjustment for each platform
#if UNITY_STANDALONE
correction = Matrix4x4.Scale(new Vector3(6.5f, 6.2f, 9.3f));
// this.transform.localScale = Vector3.one * 30;
this.transform.localScale = this.transform.localScale * 30;
#elif UNITY_IPHONE
correction = Matrix4x4.Scale(new Vector3(15.2f, 15.2f, 9.1f));
this.transform.localScale = this.transform.localScale * 30;
#elif UNITY_ANDROID
correction = Matrix4x4.Scale(new Vector3(8.1f, 8.1f, 9.1f));
this.transform.localScale = this.transform.localScale * 46;
#else
correction = Matrix4x4.Scale(new Vector3(6.5f, 6.2f, 9.3f));
this.transform.localScale = this.transform.localScale * 32;
#endif
// Acquire renderer for blendshape
smr = this.GetComponentInChildren <SkinnedMeshRenderer>();
if (smr == null)
{
throw new UnityException("SkinnedMeshRenderer for this model is missing");
}
filter = new Kalman(kalmanRatio);
}
void Awake()
{
KalmanPitch = new Kalman();
KalmanRoll = new Kalman();
dataCollector = new DataCollector();
dataCollector.Start();
}
//Setup Kalman Filter and predict methods
public void InitializeKalmanFilter()
{
_kalmanXYZ = new Kalman(6, 3, 0);
Matrix <float> state = new Matrix <float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
});
_kalmanXYZ.CorrectedState = state;
_kalmanXYZ.TransitionMatrix = new Matrix <float>(new float[, ]
{
// x-pos, y-pos, z-pos, x-velocity, y-velocity, z-velocity
{ 1, 0, 0, 1, 0, 0 },
{ 0, 1, 0, 0, 1, 0 },
{ 0, 0, 1, 0, 0, 1 },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 1, 0 },
{ 0, 0, 0, 0, 0, 1 }
});
_kalmanXYZ.MeasurementNoiseCovariance = new Matrix <float>(3, 3); //Fixed accordiong to input data
_kalmanXYZ.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1.0e-1));
_kalmanXYZ.ProcessNoiseCovariance = new Matrix <float>(6, 6); //Linked to the size of the transition matrix
_kalmanXYZ.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1.0e-4)); //The smaller the value the more resistance to noise
_kalmanXYZ.ErrorCovariancePost = new Matrix <float>(6, 6); //Linked to the size of the transition matrix
_kalmanXYZ.ErrorCovariancePost.SetIdentity();
_kalmanXYZ.MeasurementMatrix = new Matrix <float>(new float[, ]
{
{ 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 }
});
}
public Optimization(int[] n, double s, int d)
{
this.kalman = new Kalman(Variable.getVariance(), Variable.getPredeterminedVariance(), Average(n));
this.K = new double[n.Length];
this.R = n;
this.SideS = s;
this.D = d;
for (int i = 0; i < R.Length; i++)
{
K[1] = R[i];
}
}
public Matrix <float> FilterPoints(Kalman kalman, float x, float y, float z)
{
Matrix <float> prediction = kalman.Predict();
Matrix <float> estimated = kalman.Correct(new Matrix <float>(new[] { x, y, z }));
Matrix <float> results = new Matrix <float>(new[, ]
{
{ prediction[0, 0], prediction[1, 0], prediction[2, 0] },
{ estimated[0, 0], estimated[1, 0], estimated[2, 0] }
});
return(results);
}
private void inicializarKalman()
{
kal = new Kalman(4, 2, 0);
kal.TransitionMatrix = new Matrix <float>(new float[4, 4] {
{ 1, 0, 1, 0 }, { 0, 1, 0, 1 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 }
});
kal.MeasurementMatrix.SetIdentity();
kal.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
kal.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
kal.ErrorCovariancePost.SetIdentity(new MCvScalar(1e-4));
medidaKalman = new Matrix <float>(2, 1);
}
public override void UpdateState(Vector3 point)
{
componentCount = Math.Max(1, componentCount);
if (Kalman != null &&
componentCount == ComponentCount &&
Mathf.Approximately(measurementNoise, mNoise) &&
processNoiseScale == procNoiseK &&
Mathf.Approximately(processNoiseExponent, procNoiseE))
{
Kalman.Correct(point);
}
else
{
ComponentCount = componentCount;
Dimension = VectorSize * ComponentCount;
mNoise = measurementNoise;
procNoiseK = processNoiseScale;
procNoiseE = processNoiseExponent;
var initialState = new Vector3[ComponentCount];
initialState[0] = lastPoint ?? point;
Kalman = new DiscreteKalmanFilter <Vector3[], Vector3>(
initialState,
GetProcessNoise(_ => processNoiseScale),
(int)VectorSize,
0,
ToArray,
ToVector3s,
ToArray)
{
ProcessNoise = GetProcessNoise(i =>
Mathf.Pow(processNoiseScale, processNoiseExponent * (i + 1))),
MeasurementMatrix = MakePositionMeasurementMatrix(),
TransitionMatrix = MakeTransitionMatrix(),
MeasurementNoise = Matrix.Diagonal((int)VectorSize, (double)measurementNoise)
};
if (lastPoint != null)
{
Kalman.Correct(point);
}
Kalman.Predict();
lastPoint = point;
}
}
public void KalmanFilter()
{
mousePoints = new List<PointF>();
kalmanPoints = new List<PointF>();
kal = new Kalman(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;
}
public KalmanTracker()
{
kalmanPoints = new List <PointF>();
kal = new Kalman(4, 2, 0);
syntheticData = new Kalman_Filter.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;
}
/// <summary>
/// Resets the AI to be ready for a new round
/// </summary>
public override void onRestart()
{
base.onRestart();
if (p != null && p.id == 1)
{
kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
1, 1);
}
else
{
kalman = new Kalman(new Vector4(player.transform.position.x, 0, player.transform.position.y, 0),
0.1, 0.1);
}
posHistory = new Vector2[60];
posHistoryIterator = 0;
stateTimer = Time.time;
IAState = (p != null && p.id == 0) ? SWIAState.RANDOM : SWIAState.DODGE_ATTACKS;
IAAction = (p != null && p.id == 0) ? SWIAAction.MOVE_TURN_LEFT : SWIAAction.MOVE_FORWARD;
actionNumber = 0;
}
public KalmanFilterTrack()
{
//initialize new kalman filter with appropriate number of parameters
kal = new Kalman(6, 3, 0);
mk = new ModelKalman();
Matrix <float> state = new Matrix <float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
});
//define kalman filter according with the kalman model
kal.CorrectedState = state;
kal.TransitionMatrix = mk.transitionMatrix;
kal.MeasurementNoiseCovariance = mk.measurementNoise;
kal.ProcessNoiseCovariance = mk.processNoise;
kal.ErrorCovariancePost = mk.errorCovariancePost;
kal.MeasurementMatrix = mk.measurementMatrix;
start = true;
pars = new float[3];
}
//手势区域卡尔曼滤波初始化
public MyKalman(int m, int n)
{
this.m = m;
this.n = n;
kalman = new Kalman(m, n, 0);
if (m == 6)
{
InitHand();
}
else
{
InitGesture();
}
kalman.CorrectedState = state;
kalman.TransitionMatrix = this.transitionMatrix;
kalman.MeasurementNoiseCovariance = this.measurementNoise;
kalman.ProcessNoiseCovariance = this.processNoise;
kalman.ErrorCovariancePost = this.errorCovariancePost;
kalman.MeasurementMatrix = this.measurementMatrix;
}
public Ball()
{
m_PreviousBallPositions = new List <PointF>();
m_KalmanFilter = new Kalman(4, 2, 0);
m_KalmanFilter.CorrectedState = new Matrix <float>(new float[] { 0f, 0f, 0f, 0f });
m_KalmanFilter.TransitionMatrix = new Matrix <float>(new float[, ] {
{ 1f, 0, 1, 0 }, { 0, 1f, 0, 1 }, { 0, 0, 0.85f, 0 }, { 0, 0, 0, 0.85f }
});
m_KalmanFilter.MeasurementNoiseCovariance = new Matrix <float>(new float[, ] {
{ 0.0006f, 0 }, { 0, 0.0006f }
});
m_KalmanFilter.ProcessNoiseCovariance = new Matrix <float>(new float[, ] {
{ 0.1f, 0, 0, 0 }, { 0, 0.1f, 0, 0 }, { 0, 0, 0.1f, 0 }, { 0, 0, 0, 0.1f }
});
m_KalmanFilter.ErrorCovariancePost = new Matrix <float>(new float[, ] {
{ 1f, 0, 0, 0 }, { 0, 1f, 0, 0 }, { 0, 0, 1f, 0 }, { 0, 0, 0, 1f }
});
m_KalmanFilter.MeasurementMatrix = new Matrix <float>(new float[, ] {
{ 1f, 0, 0, 0 }, { 0, 1f, 0, 0 }
});
}
public KalmanFilterTrack(bool use, bool twoflies, Photodiode pd)
{
//initialize new kalman filter with appropriate number of parameters
kal = new Kalman(6, 3, 0);
mk = new ModelKalman();
Matrix <float> state = new Matrix <float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
});
//define kalman filter according with the kalman model
kal.CorrectedState = state;
kal.TransitionMatrix = mk.transitionMatrix;
kal.MeasurementNoiseCovariance = mk.measurementNoise;
kal.ProcessNoiseCovariance = mk.processNoise;
kal.ErrorCovariancePost = mk.errorCovariancePost;
kal.MeasurementMatrix = mk.measurementMatrix;
pars = new float[3];
this.use = use;
this.twoflies = twoflies;
photodiode = pd;
}
public void KalmanFilter(int camID)
{
if (camID == 0)
{
mousePoints = new List <PointF>();
kalmanPoints = new List <PointF>();
kal = new Kalman(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;
}
else if (camID == 1)
{
mousePoints2 = new List <PointF>();
kalmanPoints2 = new List <PointF>();
kal2 = new Kalman(4, 2, 0);
syntheticData2 = new SyntheticData();
Matrix <float> state2 = new Matrix <float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f
});
kal2.CorrectedState = state2;
kal2.TransitionMatrix = syntheticData2.transitionMatrix;
kal2.MeasurementNoiseCovariance = syntheticData2.measurementNoise;
kal2.ProcessNoiseCovariance = syntheticData2.processNoise;
kal2.ErrorCovariancePost = syntheticData2.errorCovariancePost;
kal2.MeasurementMatrix = syntheticData2.measurementMatrix;
}
}
public void TestKalman()
{
Image <Bgr, Byte> img = new Image <Bgr, byte>(400, 400);
SyntheticData syntheticData = new SyntheticData();
// state is (phi, delta_phi) - angle and angle increment
Matrix <float> state = new Matrix <float>(new float[] { 0.0f, 0.0f }); //initial guess
#region initialize Kalman filter
Kalman tracker = new Kalman(2, 1, 0);
tracker.TransitionMatrix = syntheticData.TransitionMatrix;
tracker.MeasurementMatrix = syntheticData.MeasurementMatrix;
tracker.ProcessNoiseCovariance = syntheticData.ProcessNoise;
tracker.MeasurementNoiseCovariance = syntheticData.MeasurementNoise;
tracker.ErrorCovariancePost = syntheticData.ErrorCovariancePost;
tracker.CorrectedState = state;
#endregion
System.Converter <double, PointF> angleToPoint =
delegate(double radianAngle)
{
return(new PointF(
(float)(img.Width / 2 + img.Width / 3 * Math.Cos(radianAngle)),
(float)(img.Height / 2 - img.Width / 3 * Math.Sin(radianAngle))));
};
Action <PointF, Bgr> drawCross =
delegate(PointF point, Bgr color)
{
img.Draw(new Cross2DF(point, 15, 15), color, 1);
};
ImageViewer viewer = new ImageViewer();
System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
timer.Interval = 200;
timer.Tick += new EventHandler(delegate(object sender, EventArgs e)
{
Matrix <float> measurement = syntheticData.GetMeasurement();
// adjust Kalman filter state
tracker.Correct(measurement);
tracker.Predict();
#region draw the state, prediction and the measurement
PointF statePoint = angleToPoint(tracker.CorrectedState[0, 0]);
PointF predictPoint = angleToPoint(tracker.PredictedState[0, 0]);
PointF measurementPoint = angleToPoint(measurement[0, 0]);
img.SetZero(); //clear the image
drawCross(statePoint, new Bgr(Color.White)); //draw current state in White
drawCross(measurementPoint, new Bgr(Color.Red)); //draw the measurement in Red
drawCross(predictPoint, new Bgr(Color.Green)); //draw the prediction (the next state) in green
img.Draw(new LineSegment2DF(statePoint, predictPoint), new Bgr(Color.Magenta), 1); //Draw a line between the current position and prediction of next position
//Trace.WriteLine(String.Format("Velocity: {0}", tracker.CorrectedState[1, 0]));
#endregion
syntheticData.GoToNextState();
viewer.Image = img;
});
timer.Start();
viewer.Disposed += delegate(Object sender, EventArgs e) { timer.Stop(); };
viewer.Text = "Actual State: White; Measurement: Red; Prediction: Green";
viewer.ShowDialog();
}
// Get the GPS data from the file
public static GPSDataList ReadGPS(string _file, GPSBox _gpsBox)
{
GPSDataList GPSDataList = new GPSDataList();
try
{
// Open the file in Read mode
FileStream fin = new FileStream(_file, FileMode.Open, FileAccess.Read, FileShare.ReadWrite);
// Seek to the end of the "GPS " boc
// Skip the first 16 'header' bits
Streams.Seek(fin, _gpsBox.Pos + 16);
int gps_read = 16;
// While we are reading in the sub box
while (gps_read < _gpsBox.Size - 8)
{
// Get the GPS box position and size
int gps_box_pos = (int)Streams.ReadUInt(fin, true);
int gps_box_size = (int)Streams.ReadUInt(fin, true);
// Update our position
gps_read = gps_read + 8;
// Save the current file seek position
long pos = fin.Position;
// Seek to this GPS data
fin.Seek(gps_box_pos, SeekOrigin.Begin);
// Get the GPS box, size, type and magic
int gps_box_size1 = (int)Streams.ReadUInt(fin, true);
string gps_box_type = Streams.ReadString(fin, 4);
string gps_box_magic = Streams.ReadString(fin, 4);
// Verify the read data is GPS data
if (gps_box_size != gps_box_size1 || gps_box_type != "free" || gps_box_magic != "GPS ")
{
// Log incorrect data format
Log.WriteLine("ERROR", "Box at position: " + gps_box_pos + " is not the expected format\r\n\t\t\t\t" +
"(Expected size: " + gps_box_size + ", Actual size: " + gps_box_size1 + ", " +
"Expected type: free, Actual type: " + gps_box_type + ", " +
"Expected magic: GPS , Actual magic: " + gps_box_magic + ")\r\n");
continue;
}
// Extract GPS data
int version = (int)Streams.ReadUInt(fin);
// Maybe able to identify firmware version with this
// Firmware 1.1 and 2.02 = 76
// Firmware 2.0 = 15344
// Firmware 2.01 - NOT COMPATIBLE, BUG IN FIRMWARE
if (version == 76)
{
// Firmware 1.1 or 2.02
fin.Seek(32, SeekOrigin.Current);
}
else if (version == 15344)
{
// Firmware 2.0
// Nothing to do
}
else
{
// Unknown Firmware Version
}
int hour = (int)Streams.ReadUInt(fin);
int minute = (int)Streams.ReadUInt(fin);
int second = (int)Streams.ReadUInt(fin);
int year = (int)Streams.ReadUInt(fin);
int month = (int)Streams.ReadUInt(fin);
int day = (int)Streams.ReadUInt(fin);
string active = Streams.ReadString(fin, 1);
string latitude_b = Streams.ReadString(fin, 1);
string longitude_b = Streams.ReadString(fin, 1);
string unknown2 = Streams.ReadString(fin, 1);
float latitude = Streams.ReadFloat(fin);
float longitude = Streams.ReadFloat(fin);
// Check the GPS had a fix
if (active == "A")
{
// Correct the speed during load
// 1 knot = 0.514444 m/s
float speed = Streams.ReadFloat(fin) * (float)0.514444;
DateTime dateTime;
if (!Enumerable.Range(1, 31).Contains(day))
{
// TODO: Calculate day from somewhere
// Firmware 1.1 has an issue with day, used to get it from file name?
}
// Create DateTime object
if (year != 0 && month != 0 && day != 0)
{
dateTime = new DateTime((year + 2000), month, day, hour, minute, second);
}
else
{
// Date not valid
dateTime = new DateTime(1);
continue;
}
// Change coordinate from DDDmm.mmmm format to traditional lat/long
double flatitude = GPSHelpers.FixCoordinates(latitude_b, latitude);
double flongitude = GPSHelpers.FixCoordinates(longitude_b, longitude);
// Create new GPSData object
GPSData gps = new GPSData();
gps.DateTime = dateTime;
gps.Speed = speed;
// Kalman filter the GPS data to remove jumps
Kalman k = new Kalman(gps.Speed);
k.Process(flatitude, flongitude, 9.9f, 1000);
gps.Latitude = k.get_lat();
gps.Longitude = k.get_lng();
if (lastLat != 0 && lastLong != 0)
{
gps.Distance = GPSHelpers.Distance(lastLat, lastLong, flatitude, flongitude);
gps.Heading = GPSHelpers.Heading(lastLat, lastLong, flatitude, flongitude);
}
lastLat = gps.Latitude;
lastLong = gps.Longitude;
if (!slowSpeed)
{
GPSDataList.FilteredGPSData.Add(gps);
}
GPSDataList.AllGPSData.Add(gps);
// Remove a few points when speed is low to stop jagged lines
if (speed <= 1)
{
slowSpeed = true;
}
else
{
slowSpeed = false;
}
}
else
{
Log.WriteVerboseLine("GPS Location is not locked. Trying next block.");
}
// Return to original file seek
fin.Seek(pos, SeekOrigin.Begin);
}
}
catch (Exception e)
{
// Unable to open file
Log.WriteLine("ERROR", "Unable to process file: " + _file);
Log.WriteLine("ERROR", e.Message.ToString());
}
return(GPSDataList);
}
