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