public GPSINSFilter(TimeSpan startTime, Vector3 startPos, Vector3 startVelocity, Quaternion orientation,
SensorSpecifications sensorSpecifications)
{
_startTime = startTime;
_orientation = orientation;
_sensorSpecifications = sensorSpecifications;
X0 = Matrix.Create(new double[n, 1]
{
{ startPos.X }, { startPos.Y }, { startPos.Z }, // Position
{ startVelocity.X }, { startVelocity.Y }, { startVelocity.Z }, // Velocity
{ _orientation.X }, { _orientation.Y }, { _orientation.Z }, { _orientation.W }, // Quaternion
});
// Make sure we don't reference the same object, but rather copy its values.
// It is possible to set PostX0 to a different state than X0, so that the initial guess
// of state is wrong.
PostX0 = X0.Clone();
// We use a very low initial estimate for error covariance, meaning the filter will
// initially trust the model more and the sensors/observations less and gradually adjust on the way.
// Note setting this to zero matrix will cause the filter to infinitely distrust all observations,
// so always use a close-to-zero value instead.
// Setting it to a diagnoal matrix of high values will cause the filter to trust the observations more in the beginning,
// since we say that we think the current PostX0 estimate is unreliable.
PostP0 = 0.001 * Matrix.Identity(n, n);
// Determine standard deviation of estimated process and observation noise variance
// Process noise (acceleromters, gyros, etc..)
_stdDevW = new Vector(new double[n]
{
_sensorSpecifications.AccelerometerStdDev.Forward,
_sensorSpecifications.AccelerometerStdDev.Right,
_sensorSpecifications.AccelerometerStdDev.Up,
0, 0, 0, 0, 0, 0, 0
});
// Observation noise (GPS inaccuarcy etc..)
_stdDevV = new Vector(new double[m]
{
_sensorSpecifications.GPSPositionStdDev.X,
_sensorSpecifications.GPSPositionStdDev.Y,
_sensorSpecifications.GPSPositionStdDev.Z,
// 0.001000, 0.001000, 0.001000,
// 1000, 1000, 1000,
_sensorSpecifications.GPSVelocityStdDev.X,
_sensorSpecifications.GPSVelocityStdDev.Y,
_sensorSpecifications.GPSVelocityStdDev.Z,
});
I = Matrix.Identity(n, n);
_zeroMM = Matrix.Zeros(m);
_rand = new GaussianRandom();
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}
private void setMatrixColumn(ref MathNet.Numerics.LinearAlgebra.Matrix m, int idx, double[] vals)
{
for (int i = 0; i < m.RowCount; ++i)
{
m[i, idx] = vals[i];
}
}
public NewtonSystem(QpProblem data, Variables initialPoint)
{
this.Q = data.Q;
this.A = data.A;
this.initialCholeskyFactor = this.Factorize(initialPoint);
}
/// <summary>
/// Creates a new Discrete Time Kalman Filter with the given values for
/// the initial state and the covariance of that state.
/// </summary>
/// <param name="x0">The best estimate of the initial state of the estimate.</param>
/// <param name="P0">The covariance of the initial state estimate. If unsure
/// about initial state, set to a large value</param>
public DiscreteKalmanFilter(Matrix<double> x0, Matrix<double> P0)
{
KalmanFilter.CheckInitialParameters(x0, P0);
x = x0;
P = P0;
}
/// <summary>
/// Creates an Information filter with the given initial state.
/// </summary>
/// <param name="x0">Initial estimate of state variables.</param>
/// <param name="P0">Covariance of state variable estimates.</param>
public InformationFilter(Matrix<double> x0, Matrix<double> P0)
{
KalmanFilter.CheckInitialParameters(x0, P0);
J = P0.Inverse();
y = J*x0;
I = Matrix<double>.Build.DenseIdentity(y.RowCount, y.RowCount);
}
private double[] getMatrixRow(MathNet.Numerics.LinearAlgebra.Matrix m, int idx)
{
double[] res = new double[m.ColumnCount];
for (int i = 0; i < m.ColumnCount; ++i)
{
res[i] = m[idx, i];
}
return(res);
}
public NeuralNetwork(int inputNodes, int hiddenNodes)
{
Theta0 = Matrix<double>.Build.Random(inputNodes, hiddenNodes);
Theta1 = Matrix<double>.Build.Random(hiddenNodes, 1);
Delta0 = Matrix<double>.Build.Dense(inputNodes,hiddenNodes);
Delta1 = Matrix<double>.Build.Dense(hiddenNodes, 1);
// (hiddenNodes, inputNodes);
}
public Residuals(QpProblem data, Variables iterate)
{
this.Q = data.Q;
this.c = data.c;
this.A = data.A;
this.b = data.b;
this.Initialise();
this.Update(iterate);
}
/// <summary>
/// Creates a square root filter with given initial state.
/// </summary>
/// <param name="x0">Initial state estimate.</param>
/// <param name="P0">Covariance of initial state estimate.</param>
public SquareRootFilter(Matrix<double> x0, Matrix<double> P0)
{
KalmanFilter.CheckInitialParameters(x0, P0);
// Decompose the covariance matrix
Matrix<double>[] UDU = UDUDecomposition(P0);
U = UDU[0];
D = UDU[1];
x = x0;
}
private static Quaternion GetOrientation(Matrix state)
{
Vector stateVector = state.GetColumnVector(0);
return(new Quaternion(
(float)stateVector[QuaternionX],
(float)stateVector[QuaternionY],
(float)stateVector[QuaternionZ],
(float)stateVector[QuaternionW]));
}
private Matrix GetNoiseMatrix(Vector stdDev, int rows)
{
var matrixData = new double[rows, 1];
for (int r = 0; r < rows; r++)
{
matrixData[r, 0] = _rand.NextGaussian(0, stdDev[r]);
}
return(Matrix.Create(matrixData));
}
/// <summary>
/// Transform sensor measurements to output matrix
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
private static Matrix ToOutputMatrix(GPSObservation s)
{
return(Matrix.Create(new double[m, 1]
{
{ s.Position.X },
{ s.Position.Y },
{ s.Position.Z },
// {0}, {0}, {0}, // Velocity
// {0}, {0}, {0}, // Acceleration
// {s.Time.TotalSeconds},
}));
}
private static GPSINSOutput ObservationMatrixToFilterResult(Matrix z)
{
Vector observationVector = z.GetColumnVector(0);
return(new GPSINSOutput
{
Position =
new Vector3((float)observationVector[ObsPositionX],
(float)observationVector[ObsPositionY],
(float)observationVector[ObsPositionZ]),
});
}
// Observations are never used.
// public IEnumerable<StepOutput<GPSFilter2DSample>> Filter(IEnumerable<GPSObservation> samples, IEnumerable<GPSFilter2DInput> inputs)
// {
// return samples.Select(s => Filter(s, i));
// }
private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSObservation observation,
GPSFilter2DInput input)
{
TimeSpan time = observation.Time;
TimeSpan timeDelta = time - _prevEstimate.Time;
UpdateMatrices(time, timeDelta);
Matrix u = ToInputMatrix(input);
// Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter
// Calculate the state and the output
Matrix x = A * prev.X + B * u + w;
Matrix z = H * x + v; //ToOutputMatrix(observation) + v;//H * x + v;
// Predictor equations
Matrix PriX = A * prev.PostX + B * u; // n by 1
Matrix AT = Matrix.Transpose(A);
Matrix PriP = A * prev.PostP * AT + Q; // n by n
Matrix residual = z - H * PriX;
Matrix residualP = H * PriP * Matrix.Transpose(H) + R;
Matrix residualPInv = residualP.Inverse();
// Corrector equations
Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m
// TODO Temp, experimenting with skipping measurements
// compileerror
// k[PositionY, PositionY] = 1.0;
// k[VelocityY, VelocityY] = 1.0;
// k[AccelerationY, AccelerationY] = 1.0;
Matrix PostX = PriX + K * residual; // n by 1
Matrix PostP = (I - K * H) * PriP; // n by n
return(new StepOutput <Matrix>
{
K = K,
PriX = PriX,
PriP = PriP,
PostX = PostX,
PostP = PostP,
PostXError = PostX - x,
PriXError = PriX - x,
X = x,
Z = z,
W = w,
V = v,
Time = time,
});
}
/// <summary>
/// Transform sensor measurements to output matrix
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
private static Matrix ToObservationMatrix(GPSINSObservation s)
{
return(Matrix.Create(new double[m, 1]
{
{ s.GPSPosition.X },
{ s.GPSPosition.Y },
{ s.GPSPosition.Z },
{ s.GPSHVelocity.X },
{ s.GPSHVelocity.Y },
{ s.GPSHVelocity.Z },
}));
}
private void UpdateMatrices(TimeSpan timeTotal, TimeSpan timeDelta)
{
double t = timeTotal.TotalSeconds;
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5 * dt * dt; // p=0.5at^2
double velByA = t; // v=at
// World state transition matrix
A = Matrix.Create(new double[n, n]
{
{ 1, 0, 0, posByV, 0, 0, posByA, 0, 0 }, // Px
{ 0, 1, 0, 0, posByV, 0, 0, posByA, 0 }, // Py
{ 0, 0, 1, 0, 0, posByV, 0, 0, posByA }, // Pz
{ 0, 0, 0, 1, 0, 0, velByA, 0, 0 }, // Vx
{ 0, 0, 0, 0, 1, 0, 0, velByA, 0 }, // Vy
{ 0, 0, 0, 0, 0, 1, 0, 0, velByA }, // Vz
{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Ax
{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Ay
{ 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Az
});
B = Matrix.Create(new double[n, p]
{
{ posByV, 0, 0, posByA, 0, 0 }, // Px
{ 0, posByV, 0, 0, posByA, 0 }, // Py
{ 0, 0, posByV, 0, 0, posByA }, // Pz
{ 1, 0, 0, velByA, 0, 0 }, // Vx
{ 0, 1, 0, 0, velByA, 0 }, // Vy
{ 0, 0, 1, 0, 0, velByA }, // Vz
{ 0, 0, 0, 1, 0, 0 }, // Ax
{ 0, 0, 0, 0, 1, 0 }, // Ay
{ 0, 0, 0, 0, 0, 1 }, // Az
});
// u = Matrix.Create(new double[p, 1]
// {
// {0}, // Px
// {0}, // Py
// {0}, // Pz
// {0}, // Vx
// {0}, // Vy
// {0}, // Vz
// {0}, // Ax
// {0}, // Ay
// {0}, // Az
// });
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}
/// <summary></summary>
/// <param name="x0">Initial system state</param>
/// <param name="postX0">Initial guess of system state</param>
/// <param name="postP0">Initial guess of a posteriori covariance</param>
/// <returns></returns>
private StepOutput <Matrix> GetInitialEstimate(Matrix x0, Matrix postX0, Matrix postP0)
{
var startingCondition = new StepInput <Matrix>(x0, postX0, postP0);
// TODO Is it ok to use an observation for the initial state in this manner?
var observation = new GPSINSObservation
{
GPSPosition = GetPosition(x0),
Time = _startTime
};
return(CalculateNext(startingCondition, observation, new GPSINSInput()));
}
public static double[] cameraTransform(double PointX, double PointY, double PointZ)
{
// From http://en.wikipedia.org/wiki/3D_projection#Perspective_projection :
//
// Point(X|Y|X) a{x,y,z} The point in 3D space that is to be projected.
// Cube.Camera(X|Y|X) c{x,y,z} The location of the camera.
// Cube.Theta(X|Y|X) θ{x,y,z} The rotation of the camera. When c{x,y,z}=<0,0,0>, and 0{x,y,z}=<0,0,0>, the 3D vector <1,2,0> is projected to the 2D vector <1,2>.
// Cube.Viewer(X|Y|X) e{x,y,z} The viewer's position relative to the display surface.
// Bsub(X|Y) b{x,y} The 2D projection of a.
//
// "First, we define a point DsubXYZ as a translation of point a{x,y,z} into a coordinate system defined by
// c{x,y,z}. This is achieved by subtracting c{x,y,z} from a{x,y,z} and then applying a vector rotation matrix
// using -θ{x,y,z} to the result. This transformation is often called a camera transform (note that these
// calculations assume a left-handed system of axes)."
MathNet.Numerics.LinearAlgebra.Matrix convMat1, convMat2, convMat3, convMat4, convMat41, convMat42, DsubXYZ;
double CosThetaX = Math.Cos(Camera.RotationX); double SinThetaX = Math.Sin(Camera.RotationX);
double CosThetaY = Math.Cos(Camera.RotationY); double SinThetaY = Math.Sin(Camera.RotationY);
double CosThetaZ = Math.Cos(Camera.RotationZ); double SinThetaZ = Math.Sin(Camera.RotationZ);
convMat1 = new MathNet.Numerics.LinearAlgebra.Matrix(new double[][] {
new double[] { 1, 0, 0 },
new double[] { 0, CosThetaX, ((-1)*(SinThetaX)) },
new double[] { 0, SinThetaX, CosThetaX }
});
convMat2 = new MathNet.Numerics.LinearAlgebra.Matrix(new double[][] {
new double[] { CosThetaY, 0, SinThetaY },
new double[] { 0, 1, 0 },
new double[] { ((-1)*(SinThetaY)), 0, CosThetaY }
});
convMat3 = new MathNet.Numerics.LinearAlgebra.Matrix(new double[][] {
new double[] { CosThetaZ, ((-1)*(SinThetaZ)), 0 },
new double[] { SinThetaZ, CosThetaZ, 0 },
new double[] { 0, 0, 1 }
});
convMat41 = new MathNet.Numerics.LinearAlgebra.Matrix(new double[][] {
new double[] { PointX },
new double[] { PointY },
new double[] { PointZ }
});
convMat42 = new MathNet.Numerics.LinearAlgebra.Matrix(new double[][] {
new double[] { Camera.X },
new double[] { Camera.Y },
new double[] { Camera.Z }
});
convMat4 = convMat41.Clone();
convMat4.Subtract(convMat42);
DsubXYZ = ((convMat1.Multiply(convMat2)).Multiply(convMat3)).Multiply(convMat4);
double[] returnVals = new double[3];
returnVals[0] = DsubXYZ[0, 0]; returnVals[1] = DsubXYZ[1, 0]; returnVals[2] = DsubXYZ[2, 0];
return returnVals;
}
//
private static GPSFilter2DSample EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return(new GPSFilter2DSample
{
Position =
new Vector3((float)stateVector[PositionX], (float)stateVector[PositionY],
(float)stateVector[PositionZ]),
// Velocity = new Vector3((float)stateVector[VelocityX], (float)stateVector[VelocityY], (float)stateVector[VelocityZ]),
// Acceleration = new Vector3((float)stateVector[AccelerationX], (float)stateVector[AccelerationY], (float)stateVector[AccelerationZ]),
// Time = TimeSpan.FromSeconds(stateVector[Time]),
});
}
private static Matrix ToInputMatrix(GPSFilter2DInput input)
{
return(Matrix.Create(new double[p, 1]
{
// {0}, // Px
// {0}, // Py
// {0}, // Pz
{ input.Velocity.X }, // Vx
{ input.Velocity.Y }, // Vy
{ input.Velocity.Z }, // Vz
{ input.Acceleration.X }, // Ax
{ input.Acceleration.Y }, // Ay
{ input.Acceleration.Z }, // Az
}));
}
private static GPSINSOutput EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return(new GPSINSOutput
{
Position = new Vector3(
(float)stateVector[PositionX],
(float)stateVector[PositionY],
(float)stateVector[PositionZ]),
Velocity = new Vector3(
(float)stateVector[VelocityX],
(float)stateVector[VelocityY],
(float)stateVector[VelocityZ]),
Orientation = GetOrientation(x),
});
}
public void CodeSample_LinearAlgebra_Eigen()
{
Matrix m = new Matrix(new double[][] {
new double[] { 10.0, -18.0 },
new double[] { 6.0, -11.0 }
});
ComplexVector eigenValues = m.EigenValues;
Assert.That(eigenValues[0].Real, NumericIs.AlmostEqualTo(1.0), "Re{eigenvalueA}");
Assert.That(eigenValues[0].Imag, NumericIs.AlmostEqualTo(0.0), "Im{eigenvalueA}");
Assert.That(eigenValues[1].Real, NumericIs.AlmostEqualTo(-2.0), "Re{eigenvalueB}");
Assert.That(eigenValues[1].Imag, NumericIs.AlmostEqualTo(0.0), "Im{eigenvalueB}");
Matrix eigenVectors = m.EigenVectors;
Assert.That(eigenVectors[0, 0], NumericIs.AlmostEqualTo(.8944271910, 1e-9), "eigenvectorA[0]");
Assert.That(eigenVectors[1, 0], NumericIs.AlmostEqualTo(.4472135955, 1e-9), "eigenvectorA[1]");
Assert.That(eigenVectors[0, 1], NumericIs.AlmostEqualTo(6.708203936, 1e-9), "eigenvectorB[0]");
Assert.That(eigenVectors[1, 1], NumericIs.AlmostEqualTo(4.472135956, 1e-9), "eigenvectorB[1]");
}
private Matrix ToInputMatrix(GPSINSInput input, TimeSpan time)
{
Vector3 worldAcceleration = input.AccelerationWorld;
var q = input.Orientation;
var inputOrientation = new Quaternion(q.X, q.Y, q.Z, q.W);
// Periodically set an orientation error that will increase the estimation error to
// make up for the missing orientation filtering. Perfect orientation hinders estimation error from accumulating significantly.
// TODO Use simulation time instead
if (time - _prevTimeOrientationNoiseGenerated > TimeSpan.FromSeconds(0.5f))
{
_prevTimeOrientationNoiseGenerated = time;
float angleStdDev = MathHelper.ToRadians(_sensorSpecifications.OrientationAngleNoiseStdDev);
_noisyRotation = Quaternion.CreateFromYawPitchRoll(_gaussRand.NextGaussian(0, angleStdDev),
_gaussRand.NextGaussian(0, angleStdDev),
_gaussRand.NextGaussian(0, angleStdDev));
}
var noisyOrientation = inputOrientation * _noisyRotation;
return(Matrix.Create(new double[p, 1]
{
{ worldAcceleration.X },
{ worldAcceleration.Y },
{ worldAcceleration.Z },
// {input.Orientation.X},
// {input.Orientation.Y},
// {input.Orientation.Z},
// {input.Orientation.W},
{ noisyOrientation.X },
{ noisyOrientation.Y },
{ noisyOrientation.Z },
{ noisyOrientation.W },
}));
}
public void TrainSet(IEnumerable<Example> testData)
{
double lambda = 0.0;
double m = 1.0;
var a = new Vector<double>[3];
var d = new Vector<double>[3];
Delta0.Clear();
Delta1.Clear();
foreach (var sample in testData)
{
m = (double)sample.X.Count;
a[0] = sample.X;
a[1] = Theta0.Multiply(a[0]).Map(SpecialFunctions.Logistic);
a[2] = Theta1.Multiply(a[1]).Map(SpecialFunctions.Logistic);
d[2] = a[2] - sample.Y;
d[1] = BackPropogate(Theta1, a[1], d[2]);
// d[0] = BackPropogate(Theta0, a[0], d[1]);
Delta1 = Delta1 + d[2].ToColumnMatrix().Multiply(a[1].ToColumnMatrix());
Delta0 = Delta0 + d[1].ToColumnMatrix().Multiply(a[0].ToColumnMatrix());
}
var D0 = Delta0.Multiply(1/m);// ignoring regularisation + Theta0.Multiply(lambda);
var D1 = Delta1.Multiply(1/m);
var unrolledTheta0 = (Theta0.RowCount*Theta0.ColumnCount);
var unrolledTheta1 = (Theta1.RowCount + Theta1.ColumnCount);
var thetaVec = new double[unrolledTheta0 + unrolledTheta1];
var DVec = new double[unrolledTheta0 + unrolledTheta1];
Theta0.ToColumnWiseArray().CopyTo(thetaVec,0);
Theta1.ToColumnWiseArray().CopyTo(thetaVec, unrolledTheta1);
D0.ToColumnWiseArray().CopyTo(DVec,0);
D1.ToColumnWiseArray().CopyTo(DVec,unrolledTheta1);
//lbfgsb.ComputeMin(BananaFunction, BananaFunctionGradient, initialGuess);
}
public GPSFilter2D(GPSObservation startState)
{
X0 = Matrix.Create(new double[n, 1]
{
{ startState.Position.X }, { startState.Position.Y }, { startState.Position.Z },
// Position
{ 0 }, { 0 }, { 0 }, // Velocity
{ 0 }, { 0 }, { 0 }, // Acceleration
});
PostX0 = X0.Clone();
/* Matrix.Create(new double[n, 1]
* {
* {startState.Position.X}, {startState.Position.Y}, {startState.Position.Z}, // Position
* {1}, {0}, {0}, // Velocity
* {0}, {1}, {0}, // Acceleration
* });*/
// Start by assuming no covariance between states, meaning position, velocity, acceleration and their three XYZ components
// have no correlation and behave independently. This not entirely true.
PostP0 = Matrix.Identity(n, n);
// Refs:
// http://www.romdas.com/technical/gps/gps-acc.htm
// http://www.sparkfun.com/datasheets/GPS/FV-M8_Spec.pdf
// http://onlinestatbook.com/java/normalshade.html
// Assuming GPS Sensor: FV-M8
// Cold start: 41s
// Hot start: 1s
// Position precision: 3.3m CEP (horizontal circle, half the points within this radius centred on truth)
// Position precision (DGPS): 2.6m CEP
// const float coVarQ = ;
// _r = covarianceR;
// Determine standard deviation of estimated process and observation noise variance
// Position process noise
_stdDevW = Vector.Zeros(n); //(float)Math.Sqrt(_q);
_rand = new GaussianRandom();
// Circle Error Probable (50% of the values are within this radius)
// const float cep = 3.3f;
// GPS position observation noise by standard deviation [meters]
// Assume gaussian distribution, 2.45 x CEP is approx. 2dRMS (95%)
// ref: http://www.gmat.unsw.edu.au/snap/gps/gps_survey/chap2/243.htm
// Found empirically by http://onlinestatbook.com/java/normalshade.html
// using area=0.5 and limits +- 3.3 meters
_stdDevV = new Vector(new double[m]
{
0,
ObservationNoiseStdDevY,
0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
// 0,
});
//Vector.Zeros(Observations);
//2, 2, 4.8926f);
H = Matrix.Identity(m, n);
I = Matrix.Identity(n, n);
Q = new Matrix(n, n);
R = Matrix.Identity(m, m);
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}
/// <summary>
/// Creates an Information filter specifying whether the covariance and state
/// have been 'inverted'.
/// </summary>
/// <param name="state">The initial estimate of the state of the system.</param>
/// <param name="cov">The covariance of the initial state estimate.</param>
/// <param name="inverted">Has covariance/state been converted to information
/// filter form?</param>
/// <remarks>This behaves the same as other constructors if the given boolean is false.
/// Otherwise, in relation to the given state/covariance should satisfy:<BR></BR>
/// <C>cov = J = P0 ^ -1, state = y = J * x0.</C></remarks>
public InformationFilter(Matrix<double> state, Matrix<double> cov, bool inverted)
{
KalmanFilter.CheckInitialParameters(state, cov);
if (inverted)
{
J = cov;
y = state;
}
else
{
J = cov.Inverse();
y = J*state;
}
I = Matrix<double>.Build.DenseIdentity(state.RowCount, state.RowCount);
}
void Update(double z, Matrix<double> H, double R)
{
Matrix<double> a = U.Transpose()*H.Transpose();
Matrix<double> b = D*a;
double dz = z - (H*x)[0, 0];
double alpha = R;
double gamma = 1d/alpha;
for (int j = 0; j < x.RowCount; j++)
{
double beta = alpha;
alpha = alpha + (a[j, 0]*b[j, 0]);
double lambda = -a[j, 0]*gamma;
gamma = 1d/alpha;
D[j, j] = beta*gamma*D[j, j];
for (int i = 0; i < j; i++)
{
beta = U[i, j];
U[i, j] = beta + (b[i, 0]*lambda);
b[i, 0] = b[i, 0] + (b[j, 0]*beta);
}
}
double dzs = gamma*dz;
x = x + (dzs*b);
}
private static GPSINSOutput EstimateMatrixToFilterResult(Matrix x)
{
Vector stateVector = x.GetColumnVector(0);
return new GPSINSOutput
{
Position = new Vector3(
(float) stateVector[PositionX],
(float) stateVector[PositionY],
(float) stateVector[PositionZ]),
Velocity = new Vector3(
(float) stateVector[VelocityX],
(float) stateVector[VelocityY],
(float) stateVector[VelocityZ]),
Orientation = GetOrientation(x),
};
}
/// <summary>
/// Performs a prediction of the state of the system after a given transition.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given state
/// transition matrix does not have the same number of row/columns as there
/// are variables in the state vector.</exception>
public void Predict(Matrix<double> F)
{
KalmanFilter.CheckPredictParameters(F, this);
Matrix<double> tmpG = Matrix<double>.Build.Dense(x.RowCount, 1);
Matrix<double> tmpQ = Matrix<double>.Build.Dense(1, 1, 1.0d);
Predict(F, tmpG, tmpQ);
}
/// <summary>
/// Updates the state of the system based on the given noisy measurements,
/// a description of how those measurements relate to the system, and a
/// covariance <c>Matrix</c> to describe the noise of the system.
/// </summary>
/// <param name="z">The measurements of the system.</param>
/// <param name="H">Measurement model.</param>
/// <param name="R">Covariance of measurements.</param>
/// <exception cref="System.ArgumentException">Thrown when given matrices
/// are of the incorrect size.</exception>
public void Update(Matrix<double> z, Matrix<double> H, Matrix<double> R)
{
// Diagonalize the given covariance matrix R
Matrix<double>[] UDU = UDUDecomposition(R);
Matrix<double> RU = UDU[0];
Matrix<double> RD = UDU[1];
Matrix<double> iRU = RU.Inverse();
Matrix<double> zh = iRU*z;
Matrix<double> Hh = iRU*H;
// Perform a scalar update for each measurement
for (int i = 0; i < z.RowCount; i++)
{
// Get sub-matrix of H
Matrix<double> subH = Hh.SubMatrix(i, 1, 0, Hh.ColumnCount);
Update(zh[i, 0], subH, RD[i, i]);
}
}
// private static TimeSpan GetTime(Matrix x)
// {
// return TimeSpan.FromSeconds(x[Time, 0]);
// }
private static Vector3 GetPosition(Matrix x)
{
return(new Vector3((float)x[PositionX, 0], (float)x[PositionX, 0], (float)x[PositionX, 0]));
}
private Vector<double> BackPropogate(Matrix<double> thetaL, Vector<double> activationL,
Vector<double> deltaLPlus1)
{
return
thetaL.TransposeThisAndMultiply(deltaLPlus1)
.PointwiseMultiply(activationL.PointwiseMultiply(1 - activationL));
}
// End FindTraceandNorm(double[,][,] Matrix, double[][] GlobalxVector, ref double Trace, ref double Norm)
public static bool SolveMatrix(double[][] Answer, Desertwind Solution)
{
var ConventionalMatrix = new double[Hotsun.npar, Hotsun.npar];
var ConventionalFirst = new double[Hotsun.npar, 1];
var ConventionalAnswer = new double[Hotsun.npar][];
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
ConventionalAnswer[GlobalIndex1] = new double[1];
}
double[, ][,] Matrix;
if (Hotsun.FullSecondDerivative)
{
Matrix = Solution.ExactFullMatrix;
}
else
{
Matrix = Solution.FullMatrix;
}
// Set actual matrix
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
if (Hotsun.FixedParameter[GlobalIndex1][0])
{
ConventionalFirst[GlobalIndex1, 0] = 0.0;
}
else
{
ConventionalFirst[GlobalIndex1, 0] = Solution.first[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex1][0];
}
for (int GlobalIndex2 = 0; GlobalIndex2 < Hotsun.npar; GlobalIndex2++)
{
double MatrixElement = Matrix[GlobalIndex1, GlobalIndex2][0, 0];
if (Hotsun.UseDiagonalScaling)
{
MatrixElement *= Hotsun.sqdginv[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex2][0];
}
if (GlobalIndex1 == GlobalIndex2)
{
if (Hotsun.AddMarquardtQDynamically)
{
MatrixElement += Hotsun.Q;
}
}
ConventionalMatrix[GlobalIndex1, GlobalIndex2] = MatrixElement;
} // End GlobalIndex2
} // End GlobalIndex1
// Form ConventionalMatrix-1 * ConventionalFirst
LMatrix cMatrix = LMatrix.Create(ConventionalMatrix);
LMatrix RightHandSide = LMatrix.Create(ConventionalFirst);
var Fred = new LU(cMatrix);
LMatrix MatrixAnswer = Fred.Solve(RightHandSide);
ConventionalAnswer = MatrixAnswer;
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
Answer[GlobalIndex1][0] = ConventionalAnswer[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex1][0];
}
bool success = true;
return(success);
}
private StepOutput<Matrix> CalculateNext(StepInput<Matrix> prev, GPSINSObservation observation,
GPSINSInput input)
{
TimeSpan time = observation.Time;
TimeSpan timeDelta = time - _prevEstimate.Time;
// Note: Use 0.0 and not 0 in order to use the correct constructor!
// If no GPS update is available, then use a zero matrix to ignore the values in the observation.
H = observation.GotGPSUpdate
? Matrix.Identity(m, n)
: new Matrix(m, n, 0.0);
HT = Matrix.Transpose(H);
UpdateTimeVaryingMatrices(timeDelta);
Matrix u = ToInputMatrix(input, time);
// Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter
// Calculate the state and the output
Matrix x = A*prev.X + B*u; // +w; noise is already modelled in input data
Matrix z = ToObservationMatrix(observation); //H * x + v;// m by 1
// Predictor equations
Matrix PriX = A*prev.PostX + B*u; // n by 1
Matrix PriP = A*prev.PostP*AT + Q; // n by n
Matrix residual = z - H*PriX; // m by 1
Matrix residualP = H*PriP*HT + R; // m by m
// If residualP is zero matrix then set its inverse to zero as well.
// This occurs if all observation standard deviations are zero
// and this workaround will cause the Kalman gain to trust the process model entirely.
Matrix residualPInv = Matrix.AlmostEqual(residualP, _zeroMM) // m by m
? _zeroMM
: residualP.Inverse();
// Corrector equations
Matrix K = PriP*Matrix.Transpose(H)*residualPInv; // n by m
Matrix PostX = PriX + K*residual; // n by 1
Matrix PostP = (I - K*H)*PriP; // n by n
var tmpPosition = new Vector3((float)PostX[0, 0], (float)PostX[1, 0], (float)PostX[2, 0]);
// var tmpPrevPosition = new Vector3((float)prev.PostX[0, 0], (float)prev.PostX[1, 0], (float)prev.PostX[2, 0]);
// Vector3 positionChange = tmpPosition - tmpPrevPosition;
Vector3 gpsPositionTrust = new Vector3((float)K[0, 0], (float)K[1, 1], (float)K[2, 2]);
Vector3 gpsVelocityTrust = new Vector3((float)K[3, 3], (float)K[4, 4], (float)K[5, 5]);
//
// Matrix tmpPriPGain = A*Matrix.Identity(10,10)*AT + Q;
var tmpVelocity = new Vector3((float)x[3, 0], (float)x[4, 0], (float)x[5, 0]);
var tmpAccel = new Vector3((float)x[6, 0], (float)x[7, 0], (float)x[8, 0]);
return new StepOutput<Matrix>
{
K = K,
PriX = PriX,
PriP = PriP,
PostX = PostX,
PostP = PostP,
PostXError = PostX - x,
PriXError = PriX - x,
X = x,
Z = z,
W = w,
V = v,
Time = time,
};
}
/// <summary></summary>
/// <param name="x0">Initial system state</param>
/// <param name="postX0">Initial guess of system state</param>
/// <param name="postP0">Initial guess of a posteriori covariance</param>
/// <returns></returns>
private StepOutput<Matrix> GetInitialEstimate(Matrix x0, Matrix postX0, Matrix postP0)
{
var startingCondition = new StepInput<Matrix>(x0, postX0, postP0);
// TODO Is it ok to use an observation for the initial state in this manner?
var observation = new GPSINSObservation
{
GPSPosition = GetPosition(x0),
Time = _startTime
};
return CalculateNext(startingCondition, observation, new GPSINSInput());
}
private StepOutput <Matrix> CalculateNext(StepInput <Matrix> prev, GPSINSObservation observation,
GPSINSInput input)
{
TimeSpan time = observation.Time;
TimeSpan timeDelta = time - _prevEstimate.Time;
// Note: Use 0.0 and not 0 in order to use the correct constructor!
// If no GPS update is available, then use a zero matrix to ignore the values in the observation.
H = observation.GotGPSUpdate
? Matrix.Identity(m, n)
: new Matrix(m, n, 0.0);
HT = Matrix.Transpose(H);
UpdateTimeVaryingMatrices(timeDelta);
Matrix u = ToInputMatrix(input, time);
// Ref equations: http://en.wikipedia.org/wiki/Kalman_filter#The_Kalman_filter
// Calculate the state and the output
Matrix x = A * prev.X + B * u; // +w; noise is already modelled in input data
Matrix z = ToObservationMatrix(observation); //H * x + v;// m by 1
// Predictor equations
Matrix PriX = A * prev.PostX + B * u; // n by 1
Matrix PriP = A * prev.PostP * AT + Q; // n by n
Matrix residual = z - H * PriX; // m by 1
Matrix residualP = H * PriP * HT + R; // m by m
// If residualP is zero matrix then set its inverse to zero as well.
// This occurs if all observation standard deviations are zero
// and this workaround will cause the Kalman gain to trust the process model entirely.
Matrix residualPInv = Matrix.AlmostEqual(residualP, _zeroMM) // m by m
? _zeroMM
: residualP.Inverse();
// Corrector equations
Matrix K = PriP * Matrix.Transpose(H) * residualPInv; // n by m
Matrix PostX = PriX + K * residual; // n by 1
Matrix PostP = (I - K * H) * PriP; // n by n
var tmpPosition = new Vector3((float)PostX[0, 0], (float)PostX[1, 0], (float)PostX[2, 0]);
// var tmpPrevPosition = new Vector3((float)prev.PostX[0, 0], (float)prev.PostX[1, 0], (float)prev.PostX[2, 0]);
// Vector3 positionChange = tmpPosition - tmpPrevPosition;
Vector3 gpsPositionTrust = new Vector3((float)K[0, 0], (float)K[1, 1], (float)K[2, 2]);
Vector3 gpsVelocityTrust = new Vector3((float)K[3, 3], (float)K[4, 4], (float)K[5, 5]);
//
// Matrix tmpPriPGain = A*Matrix.Identity(10,10)*AT + Q;
var tmpVelocity = new Vector3((float)x[3, 0], (float)x[4, 0], (float)x[5, 0]);
var tmpAccel = new Vector3((float)x[6, 0], (float)x[7, 0], (float)x[8, 0]);
return(new StepOutput <Matrix>
{
K = K,
PriX = PriX,
PriP = PriP,
PostX = PostX,
PostP = PostP,
PostXError = PostX - x,
PriXError = PriX - x,
X = x,
Z = z,
W = w,
V = v,
Time = time,
});
}
public GPSINSFilter(TimeSpan startTime, Vector3 startPos, Vector3 startVelocity, Quaternion orientation,
SensorSpecifications sensorSpecifications)
{
_startTime = startTime;
_orientation = orientation;
_sensorSpecifications = sensorSpecifications;
X0 = Matrix.Create(new double[n,1]
{
{startPos.X}, {startPos.Y}, {startPos.Z}, // Position
{startVelocity.X}, {startVelocity.Y}, {startVelocity.Z}, // Velocity
{_orientation.X}, {_orientation.Y}, {_orientation.Z}, {_orientation.W}, // Quaternion
});
// Make sure we don't reference the same object, but rather copy its values.
// It is possible to set PostX0 to a different state than X0, so that the initial guess
// of state is wrong.
PostX0 = X0.Clone();
// We use a very low initial estimate for error covariance, meaning the filter will
// initially trust the model more and the sensors/observations less and gradually adjust on the way.
// Note setting this to zero matrix will cause the filter to infinitely distrust all observations,
// so always use a close-to-zero value instead.
// Setting it to a diagnoal matrix of high values will cause the filter to trust the observations more in the beginning,
// since we say that we think the current PostX0 estimate is unreliable.
PostP0 = 0.001*Matrix.Identity(n, n);
// Determine standard deviation of estimated process and observation noise variance
// Process noise (acceleromters, gyros, etc..)
_stdDevW = new Vector(new double[n]
{
_sensorSpecifications.AccelerometerStdDev.Forward,
_sensorSpecifications.AccelerometerStdDev.Right,
_sensorSpecifications.AccelerometerStdDev.Up,
0, 0, 0, 0, 0, 0, 0
});
// Observation noise (GPS inaccuarcy etc..)
_stdDevV = new Vector(new double[m]
{
_sensorSpecifications.GPSPositionStdDev.X,
_sensorSpecifications.GPSPositionStdDev.Y,
_sensorSpecifications.GPSPositionStdDev.Z,
// 0.001000, 0.001000, 0.001000,
// 1000, 1000, 1000,
_sensorSpecifications.GPSVelocityStdDev.X,
_sensorSpecifications.GPSVelocityStdDev.Y,
_sensorSpecifications.GPSVelocityStdDev.Z,
});
I = Matrix.Identity(n, n);
_zeroMM = Matrix.Zeros(m);
_rand = new GaussianRandom();
_prevEstimate = GetInitialEstimate(X0, PostX0, PostP0);
}
/// <summary>
/// Perform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given state
/// transition matrix does not have the same number of row/columns as there
/// are variables in the state vector.</exception>
public void Predict(Matrix<double> F)
{
KalmanFilter.CheckPredictParameters(F, this);
// Easier just to convert back to discrete form....
Matrix<double> p = J.Inverse();
Matrix<double> x = p*y;
x = F*x;
p = F*p*F.Transpose();
J = p.Inverse();
y = J*x;
}
/// <summary>
/// Performs a prediction of the state of the system after a given transition.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="G">Noise coupling matrix.</param>
/// <param name="Q">Noise covariance matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when the given matrices
/// are not the correct dimensions.</exception>
public void Predict(Matrix<double> F, Matrix<double> G, Matrix<double> Q)
{
KalmanFilter.CheckPredictParameters(F, G, Q, this);
// Update the state.. that is easy!!
x = F*x;
// Get all the sized and create storage
int n = x.RowCount;
int p = G.ColumnCount;
Matrix<double> outD = Matrix<double>.Build.Dense(n, n); // Updated diagonal matrix
Matrix<double> outU = Matrix<double>.Build.DenseIdentity(n, n); // Updated upper unit triangular
// Get the UD Decomposition of the process noise matrix
Matrix<double>[] UDU = UDUDecomposition(Q);
Matrix<double> Uq = UDU[0];
Matrix<double> Dq = UDU[1];
// Combine it with the noise coupling matrix
Matrix<double> Gh = G*Uq;
// Convert state transition matrix
Matrix<double> PhiU = F*U;
// Ready to go..
for (int i = n - 1; i >= 0; i--)
{
// Update the i'th diagonal of the covariance matrix
double sigma = 0.0d;
for (int j = 0; j < n; j++)
{
sigma = sigma + (PhiU[i, j]*PhiU[i, j]*D[j, j]);
}
for (int j = 0; j < p; j++)
{
sigma = sigma + (Gh[i, j]*Gh[i, j]*Dq[j, j]);
}
outD[i, i] = sigma;
// Update the i'th row of the upper triangular of covariance
for (int j = 0; j < i; j++)
{
sigma = 0.0d;
for (int k = 0; k < n; k++)
{
sigma = sigma + (PhiU[i, k]*D[k, k]*PhiU[j, k]);
}
for (int k = 0; k < p; k++)
{
sigma = sigma + (Gh[i, k]*Dq[k, k]*Gh[j, k]);
}
outU[j, i] = sigma/outD[i, i];
for (int k = 0; k < n; k++)
{
PhiU[j, k] = PhiU[j, k] - (outU[j, i]*PhiU[i, k]);
}
for (int k = 0; k < p; k++)
{
Gh[j, k] = Gh[j, k] - (outU[j, i]*Gh[i, k]);
}
}
}
// Update the covariance
U = outU;
D = outD;
}
/// <summary>
/// Preform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="Q">A plant noise covariance matrix.</param>
/// <exception cref="System.ArgumentException">Thrown when F and Q are not
/// square matrices with the same number of rows and columns as there are
/// rows in the state matrix.</exception>
/// <remarks>Performs a prediction of the next state of the Kalman Filter,
/// where there is plant noise. The covariance matrix of the plant noise, in
/// this case, is a square matrix corresponding to the state transition and
/// the state of the system.</remarks>
public void Predict(Matrix<double> F, Matrix<double> Q)
{
// We will need these matrices more than once...
Matrix<double> FI = F.Inverse();
Matrix<double> FIT = FI.Transpose();
Matrix<double> A = FIT*J*FI;
Matrix<double> AQI = (A + Q.Inverse()).Inverse();
// 'Covariance' Update
J = A - (A*AQI*A);
y = (I - (A*AQI))*FIT*y;
}
/// <summary>
/// Perform a discrete time prediction of the system state.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="G">Noise coupling matrix.</param>
/// <param name="Q">Plant noise covariance.</param>
/// <exception cref="System.ArgumentException">Thrown when the column and row
/// counts for the given matrices are incorrect.</exception>
/// <remarks>
/// Performs a prediction of the next state of the Kalman Filter, given
/// a description of the dynamic equations of the system, the covariance of
/// the plant noise affecting the system and the equations that describe
/// the effect on the system of that plant noise.
/// </remarks>
public void Predict(Matrix<double> F, Matrix<double> G, Matrix<double> Q)
{
// Some matrices we will need a bit
Matrix<double> FI = F.Inverse();
Matrix<double> FIT = FI.Transpose();
Matrix<double> GT = G.Transpose();
Matrix<double> A = FIT*J*FI;
Matrix<double> B = A*G*(GT*A*G + Q.Inverse()).Inverse();
J = (I - B*GT)*A;
y = (I - B*GT)*FIT*y;
}
private void UpdateTimeVaryingMatrices(TimeSpan timeDelta)
{
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5 * dt * dt; // p=0.5at^2
double velByA = dt; // v=at
// World state transition matrix.
// Update position and velocity from previous state.
// Previous state acceleration is neglected since current acceleration only depends on current input.
A = Matrix.Create(new double[n, n]
{
// Px Py Pz Vx Vy Vz Qx Qy Qz Qw
{ 1, 0, 0, posByV, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, 1, 0, 0, posByV, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, 1, 0, 0, posByV, 0, 0, 0, 0 }, // Pz
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }, // Vz
// We don't handle transition of quaternions here due to difficulties. Using B instead.
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion W
});
AT = Matrix.Transpose(A);
// Input gain matrix.
// Acceleration forward/right/down
// Angular Rate Roll/Pitch/Heading
B = Matrix.Create(new double[n, p]
{
// Ax Ay Az Qx Qy Qz Qw
{ posByA, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, posByA, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, posByA, 0, 0, 0, 0 }, // Pz
{ velByA, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, velByA, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, velByA, 0, 0, 0, 0 }, // Vz
// Simply set new orientation directly by quaternion input
{ 0, 0, 0, 1, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 1, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 1, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 1 }, // Quaternion W
});
// TODO For simplicity we assume all acceleromter axes have identical standard deviations (although they don't)
float accelStdDev = _sensorSpecifications.AccelerometerStdDev.Forward;
float velocityStdDev = ((float)velByA) * accelStdDev;
float positionStdDev = ((float)posByA) * accelStdDev;
// Diagonal matrix with noise std dev
Q = Matrix.Diagonal(new Vector(new double[n]
{
positionStdDev, positionStdDev, positionStdDev,
velocityStdDev, velocityStdDev, velocityStdDev,
0, 0, 0, 0 // TODO Orientation has no noise, should be added later
}));
R = Matrix.Diagonal(_stdDevV); // Diagonal matrix with noise std dev
Q *= Matrix.Transpose(Q); // Convert standard deviations to variances
R *= Matrix.Transpose(R); // Convert standard deviations to variances
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}
/// <summary>
/// Updates the state of the system based on the given noisy measurements,
/// a description of how those measurements relate to the system, and a
/// covariance <c>Matrix</c> to describe the noise of the system.
/// </summary>
/// <param name="z">The measurements of the system.</param>
/// <param name="H">Measurement model.</param>
/// <param name="R">Covariance of measurements.</param>
/// <exception cref="System.ArgumentException">Thrown when given matrices
/// are of the incorrect size.</exception>
public void Update(Matrix<double> z, Matrix<double> H, Matrix<double> R)
{
KalmanFilter.CheckUpdateParameters(z, H, R, this);
// Fiddle with the matrices
Matrix<double> HT = H.Transpose();
Matrix<double> RI = R.Inverse();
// Perform the update
y = y + (HT*RI*z);
J = J + (HT*RI*H);
}
private void UpdateTimeVaryingMatrices(TimeSpan timeDelta)
{
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5*dt*dt; // p=0.5at^2
double velByA = dt; // v=at
// World state transition matrix.
// Update position and velocity from previous state.
// Previous state acceleration is neglected since current acceleration only depends on current input.
A = Matrix.Create(new double[n,n]
{
// Px Py Pz Vx Vy Vz Qx Qy Qz Qw
{1, 0, 0, posByV, 0, 0, 0, 0, 0, 0}, // Px
{0, 1, 0, 0, posByV, 0, 0, 0, 0, 0}, // Py
{0, 0, 1, 0, 0, posByV, 0, 0, 0, 0}, // Pz
{0, 0, 0, 1, 0, 0, 0, 0, 0, 0}, // Vx
{0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, // Vy
{0, 0, 0, 0, 0, 1, 0, 0, 0, 0}, // Vz
// We don't handle transition of quaternions here due to difficulties. Using B instead.
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, // Quaternion X
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, // Quaternion Y
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, // Quaternion Z
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, // Quaternion W
});
AT = Matrix.Transpose(A);
// Input gain matrix.
// Acceleration forward/right/down
// Angular Rate Roll/Pitch/Heading
B = Matrix.Create(new double[n,p]
{
// Ax Ay Az Qx Qy Qz Qw
{posByA, 0, 0, 0, 0, 0, 0}, // Px
{0, posByA, 0, 0, 0, 0, 0}, // Py
{0, 0, posByA, 0, 0, 0, 0}, // Pz
{velByA, 0, 0, 0, 0, 0, 0}, // Vx
{0, velByA, 0, 0, 0, 0, 0}, // Vy
{0, 0, velByA, 0, 0, 0, 0}, // Vz
// Simply set new orientation directly by quaternion input
{0, 0, 0, 1, 0, 0, 0}, // Quaternion X
{0, 0, 0, 0, 1, 0, 0}, // Quaternion Y
{0, 0, 0, 0, 0, 1, 0}, // Quaternion Z
{0, 0, 0, 0, 0, 0, 1}, // Quaternion W
});
// TODO For simplicity we assume all acceleromter axes have identical standard deviations (although they don't)
float accelStdDev = _sensorSpecifications.AccelerometerStdDev.Forward;
float velocityStdDev = ((float)velByA) * accelStdDev;
float positionStdDev = ((float)posByA) * accelStdDev;
// Diagonal matrix with noise std dev
Q = Matrix.Diagonal(new Vector(new double[n]
{
positionStdDev, positionStdDev, positionStdDev,
velocityStdDev, velocityStdDev, velocityStdDev,
0, 0, 0, 0 // TODO Orientation has no noise, should be added later
}));
R = Matrix.Diagonal(_stdDevV); // Diagonal matrix with noise std dev
Q *= Matrix.Transpose(Q); // Convert standard deviations to variances
R *= Matrix.Transpose(R); // Convert standard deviations to variances
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}
/// <summary>
/// Creates an Information Filter from a given Kalman Filter.
/// </summary>
/// <param name="kf">The filter used to derive the information filter.</param>
public InformationFilter(IKalmanFilter kf)
{
J = kf.Cov.Inverse();
y = J*kf.State;
I = Matrix<double>.Build.DenseIdentity(y.RowCount, y.RowCount);
}
private static GPSINSOutput ObservationMatrixToFilterResult(Matrix z)
{
Vector observationVector = z.GetColumnVector(0);
return new GPSINSOutput
{
Position =
new Vector3((float) observationVector[ObsPositionX],
(float) observationVector[ObsPositionY],
(float) observationVector[ObsPositionZ]),
};
}
static Matrix<double>[] UDUDecomposition(Matrix<double> Arg)
{
// Initialize some values
int n = Arg.RowCount; // Number of elements in matrix
double sigma; // Temporary value
double[,] aU = new double[n, n];
double[,] aD = new double[n, n];
for (int j = n - 1; j >= 0; j--)
{
for (int i = j; i >= 0; i--)
{
sigma = Arg[i, j];
for (int k = j + 1; k < n; k++)
{
sigma = sigma - (aU[i, k]*aD[k, k]*aU[j, k]);
}
if (i == j)
{
aD[j, j] = sigma;
aU[j, j] = 1d;
}
else
{
aU[i, j] = sigma/aD[j, j];
}
}
}
// Create the output... first Matrix is L, next is D
var outMats = new Matrix<double>[2];
outMats[0] = Matrix<double>.Build.DenseOfArray(aU);
outMats[1] = Matrix<double>.Build.DenseOfArray(aD);
return outMats;
}
private static Vector3 GetPosition(Matrix x)
{
return new Vector3((float) x[PositionX, 0], (float) x[PositionX, 0], (float) x[PositionX, 0]);
}
public void Setup()
{
// MATLAB: ma3x2 = [1 -2;-1 4;5 7]
_ma3X2 = new Matrix(new double[][] {
new double[] { 1, -2 },
new double[] { -1, 4 },
new double[] { 5, 7 }
});
// MATLAB: mb3x2 = [10 2.5;-3 -1.5;19 -6]
_mb3X2 = new Matrix(new double[][] {
new double[] { 10, 2.5 },
new double[] { -3, -1.5 },
new double[] { 19, -6 }
});
// MATLAB: mc2x2 = [1 2;3 4]
_mc2X2 = new Matrix(new double[][] {
new double[] { 1, 2 },
new double[] { 3, 4 }
});
// MATLAB: md2x4 = [1 2 -3 12;3 3.1 4 2]
_md2X4 = new Matrix(new double[][] {
new double[] { 1, 2, -3, 12 },
new double[] { 3, 3.1, 4, 2 }
});
// MATLAB: ra3x2 = ma3x2 + 2
_ra3X2 = ComplexMatrix.Create(_ma3X2) + 2;
// MATLAB: rb3x2 = mb3x2 - 1
_rb3X2 = ComplexMatrix.Create(_mb3X2) - 1;
// MATLAB: rc2x2 = mc2x2 + 5
_rc2X2 = ComplexMatrix.Create(_mc2X2) + 5;
// MATLAB: rd2x4 = md2x4 * 2
_rd2X4 = ComplexMatrix.Create(_md2X4) * 2;
// MATLAB: ia3x2 = (ra3x2 * 2) * j
_ia3X2 = (_ra3X2 * 2) * j;
// MATLAB: ib3x2 = (rb3x2 * 3 + 1) * j
_ib3X2 = ((_rb3X2 * 3) + 1) * j;
// MATLAB: ic2x2 = (rc2x2 + 2) * j
_ic2X2 = (_rc2X2 + 2) * j;
// MATLAB: id2x4 = (rd2x4 - 5) * j
_id2X4 = (_rd2X4 - 5) * j;
// MATLAB: ca3x2 = 2*ra3x2 - 2*ia3x2
_ca3X2 = (2 * _ra3X2) - (2 * _ia3X2);
// MATLAB: cb3x2 = rb3x2 + 3*ib3x2
_cb3X2 = _rb3X2 + (3 * _ib3X2);
// MATLAB: cc2x2 = rc2x2 + 2 - 3*ic2x2
_cc2X2 = _rc2X2 + 2 - (3 * _ic2X2);
// MATLAB: cd2x4 = -2*rd2x4 + id2x4 + 1-j
_cd2X4 = (-2 * _rd2X4) + _id2X4 + (1 - j);
// MATLAB: v2 = [5 -2]
_v2 = new Vector(new double[] { 5, -2 });
// MATLAB: cv2 = [5+j, -2+3j]
_cv2 = new ComplexVector(new Complex[] { 5 + j, -2 + (3 * j) });
}
private static Quaternion GetOrientation(Matrix state)
{
Vector stateVector = state.GetColumnVector(0);
return new Quaternion(
(float) stateVector[QuaternionX],
(float) stateVector[QuaternionY],
(float) stateVector[QuaternionZ],
(float) stateVector[QuaternionW]);
}
// End AddupChisqContributions(ChisqFirstandSecond[] SubTotal, Desertwind TotalSolution)
public static void FindQlimits(Desertwind Solution, ref double Qhigh, ref double Qlow, ref int ReasontoStop1,
ref int ReasontoStop2)
{
if (Hotsun.FullSecondDerivative)
{
FindTraceandNorm(Solution.ExactFullMatrix, ref Hotsun.ChisqMatrixTrace, ref Hotsun.ChisqMatrixNorm);
}
else
{
FindTraceandNorm(Solution.FullMatrix, ref Hotsun.ChisqMatrixTrace, ref Hotsun.ChisqMatrixNorm);
}
var ConventionalMatrix = new double[Hotsun.npar, Hotsun.npar];
// Set Up Matrix to find eigenvalues
// Scale but do NOT add Q
double[, ][,] Matrix;
if (Hotsun.FullSecondDerivative)
{
Matrix = Solution.ExactFullMatrix;
}
else
{
Matrix = Solution.FullMatrix;
}
// Set actual matrix
for (int GlobalIndex1 = 0; GlobalIndex1 < Hotsun.npar; GlobalIndex1++)
{
for (int GlobalIndex2 = 0; GlobalIndex2 < Hotsun.npar; GlobalIndex2++)
{
double MatrixElement = Matrix[GlobalIndex1, GlobalIndex2][0, 0];
if (Hotsun.UseDiagonalScaling)
{
MatrixElement *= Hotsun.sqdginv[GlobalIndex1][0] * Hotsun.sqdginv[GlobalIndex2][0];
}
ConventionalMatrix[GlobalIndex1, GlobalIndex2] = MatrixElement;
} // End GlobalIndex2
} // End GlobalIndex1
// Find Minimum and Maximum eigenvalue of ConventionalMatrix
// Begin Added by smbeason 5/17/2009
LMatrix lmatrix = LMatrix.Create(ConventionalMatrix);
EigenvalueDecomposition eigenValueDecomp = lmatrix.EigenvalueDecomposition;
double minEigenValue = double.MaxValue;
double maxEigenValue = double.MinValue;
//Assuming you want on the real part...
for (int i = 0; i < eigenValueDecomp.RealEigenvalues.Length; i++)
{
minEigenValue = Math.Min(minEigenValue, eigenValueDecomp.RealEigenvalues[i]);
maxEigenValue = Math.Max(maxEigenValue, eigenValueDecomp.RealEigenvalues[i]);
}
// End Added by smbeason 5/17/2009
ReasontoStop1 = 1;
ReasontoStop2 = 1;
Qlow = minEigenValue;
Qhigh = maxEigenValue;
return;
}
