/// <summary>
/// Initializes the state to the supplied observation.
/// </summary>
/// <param name="z">The z.</param>
public void Initialize(Position3D z)
{
SquareMatrix3D hi = SquareMatrix3D.Invert(_h);
_z = z.ToCartesianPoint(_ellipsoid);
//s.x = inv(s.H)*s.z;
_x = hi * _z;
//s.P = inv(s.H)*s.R*inv(s.H');
_p = hi * _r * SquareMatrix3D.Invert(SquareMatrix3D.Transpose(_h));
_lastObservation = DateTime.Now;
_delay = TimeSpan.Zero;
}
/// <summary>
/// Initializes a new instance of the <see cref="KalmanSystemState"/> struct.
/// </summary>
/// <param name="gpsPosition">The GPS position.</param>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="ellipsoid">The ellipsoid.</param>
/// <param name="u">The u.</param>
/// <param name="x">The x.</param>
/// <param name="z">The z.</param>
/// <param name="a">A.</param>
/// <param name="b">The b.</param>
/// <param name="h">The h.</param>
/// <param name="p">The p.</param>
/// <param name="q">The q.</param>
/// <param name="r">The r.</param>
internal KalmanSystemState(
Position3D gpsPosition, Distance deviceError,
DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP,
Ellipsoid ellipsoid,
CartesianPoint u, CartesianPoint x, CartesianPoint z,
SquareMatrix3D a, SquareMatrix3D b, SquareMatrix3D h,
SquareMatrix3D p, SquareMatrix3D q, SquareMatrix3D r)
{
_deviceError = deviceError.IsEmpty ? DilutionOfPrecision.CurrentAverageDevicePrecision.Value : deviceError.Value;
_horizontalDOP = horizontalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : horizontalDOP.Value;
_verticalDOP = verticalDOP.IsEmpty ? DilutionOfPrecision.Good.Value : verticalDOP.Value;
_ellipsoid = ellipsoid ?? Ellipsoid.Default;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
_u = u.IsInvalid ? CartesianPoint.Empty : u;
_x = x;
_z = z.IsInvalid ? CartesianPoint.Empty : z;
_a = a ?? new SquareMatrix3D();
_b = b ?? new SquareMatrix3D();
_h = h ?? new SquareMatrix3D();
_p = p ?? new SquareMatrix3D();
_q = q ?? SquareMatrix3D.Default(0);
_r = r ?? new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
_interval = 0;
_errorState = Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2));
_delay = TimeSpan.MaxValue;
_lastObservation = DateTime.MinValue;
if (!gpsPosition.IsEmpty)
{
Initialize(gpsPosition);
}
}
/// <summary>
/// Updates the state.
/// </summary>
/// <param name="deviceError">The device error.</param>
/// <param name="horizontalDOP">The horizontal DOP.</param>
/// <param name="verticalDOP">The vertical DOP.</param>
/// <param name="bearing">The bearing.</param>
/// <param name="speed">The speed.</param>
/// <param name="z">The z.</param>
public void UpdateState(Distance deviceError, DilutionOfPrecision horizontalDOP, DilutionOfPrecision verticalDOP, Azimuth bearing, Speed speed, Position3D z)
{
if (_x.IsInvalid)
{
Initialize(z);
return;
}
// More insanity
double fail = horizontalDOP.Value * verticalDOP.Value * deviceError.Value;
if (fail == 0 || double.IsNaN(fail) || double.IsInfinity(fail))
{
throw new ArgumentException(
"Covariance values are invalid. Parameters deviceError, horizontalDOP and verticalDOP must be greater than zero.");
}
_deviceError = deviceError.Value;
_horizontalDOP = horizontalDOP.Value;
_verticalDOP = verticalDOP.Value;
double hCovariance = _deviceError * _horizontalDOP;
double vCovariance = _deviceError * _verticalDOP;
// Setup the observation covariance (measurement error)
_r = new SquareMatrix3D(
hCovariance, 0, 0,
0, hCovariance, 0,
0, 0, vCovariance);
#region Process Noise Estimation
// Get the translation of the last correction
CartesianPoint subX = _x.ToPosition3D(_ellipsoid)
.TranslateTo(bearing, speed.ToDistance(_delay), _ellipsoid)
.ToCartesianPoint();
// Get the vector of the translation and the last observation
//CartesianPoint w = (subX - this.z);
CartesianPoint w =
new CartesianPoint(
Distance.FromMeters(subX.X.Value - _z.X.Value), // Values are in meters
Distance.FromMeters(subX.Y.Value - _z.Y.Value), // Values are in meters
Distance.FromMeters(subX.Z.Value - _z.Z.Value)); // Values are in meters
// Setup the noise covariance (process error)
_q = new SquareMatrix3D(
Math.Abs(w.X.Value), 0, 0,
0, Math.Abs(w.Y.Value), 0,
0, 0, Math.Abs(w.Z.Value));
#endregion Process Noise Estimation
// Update the observation state
_z = z.ToCartesianPoint(_ellipsoid);
#region State vector prediction and covariance
//s.x = s.A*s.x + s.B*s.u;
//this.x = this.A * this.x + this.B * this.u;
CartesianPoint ax = _a.TransformVector(_x);
CartesianPoint bu = _b.TransformVector(_u);
_x =
new CartesianPoint(
Distance.FromMeters(ax.X.Value + bu.X.Value),
Distance.FromMeters(ax.Y.Value + bu.Y.Value),
Distance.FromMeters(ax.Z.Value + bu.Z.Value));
//s.P = s.A * s.P * s.A' + s.Q;
_p = _a * _p * SquareMatrix3D.Transpose(_a) + _q;
#endregion State vector prediction and covariance
#region Kalman gain factor
//K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
SquareMatrix3D ht = SquareMatrix3D.Transpose(_h);
SquareMatrix3D k = _p * ht * SquareMatrix3D.Invert(_h * _p * ht + _r);
#endregion Kalman gain factor
#region Observational correction
//s.x = s.x + K*(s.z-s.H*s.x);
//this.x = this.x + K * (this.z - this.H * this.x);
CartesianPoint hx = _h.TransformVector(_x);
CartesianPoint zHx = new CartesianPoint(
Distance.FromMeters(_z.X.Value - hx.X.Value),
Distance.FromMeters(_z.Y.Value - hx.Y.Value),
Distance.FromMeters(_z.Z.Value - hx.Z.Value));
CartesianPoint kzHx = k.TransformVector(zHx);
_x =
new CartesianPoint(
Distance.FromMeters(_x.X.Value + kzHx.X.Value),
Distance.FromMeters(_x.Y.Value + kzHx.Y.Value),
Distance.FromMeters(_x.Z.Value + kzHx.Z.Value));
//s.P = s.P - K*s.H*s.P;
_p = _p - k * _h * _p;
#endregion Observational correction
// Bump the state count
_interval++;
// Calculate the average error for the system stste.
_errorState = (_errorState + Math.Sqrt(Math.Pow(hCovariance, 2) + Math.Pow(vCovariance, 2))) * .5f;
// Calculate the interval between samples
DateTime now = DateTime.Now;
_delay = now.Subtract(_lastObservation);
_lastObservation = now;
}
