// 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> /// 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; }
/// <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); }
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); }
/// <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; }
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); }
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, }); }
SingularValueDecomposition(IMatrix <double> arg) { _transpose = (arg.RowCount < arg.ColumnCount); // Derived from LINPACK code. // Initialize. double[][] a; if (_transpose) { // copy of internal data, independent of Arg a = Matrix.Transpose(arg).GetArray(); _m = arg.ColumnCount; _n = arg.RowCount; } else { a = arg.CopyToJaggedArray(); _m = arg.RowCount; _n = arg.ColumnCount; } int nu = Math.Min(_m, _n); double[] s = new double[Math.Min(_m + 1, _n)]; double[][] u = Matrix.CreateMatrixData(_m, nu); double[][] v = Matrix.CreateMatrixData(_n, _n); double[] e = new double[_n]; double[] work = new double[_m]; /* * Reduce A to bidiagonal form, storing the diagonal elements * in s and the super-diagonal elements in e. */ int nct = Math.Min(_m - 1, _n); int nrt = Math.Max(0, Math.Min(_n - 2, _m)); for (int k = 0; k < Math.Max(nct, nrt); k++) { if (k < nct) { // Compute the transformation for the k-th column and // place the k-th diagonal in s[k]. // Compute 2-norm of k-th column without under/overflow. s[k] = 0; for (int i = k; i < _m; i++) { s[k] = Fn.Hypot(s[k], a[i][k]); } if (s[k] != 0.0) { if (a[k][k] < 0.0) { s[k] = -s[k]; } for (int i = k; i < _m; i++) { a[i][k] /= s[k]; } a[k][k] += 1.0; } s[k] = -s[k]; } for (int j = k + 1; j < _n; j++) { if ((k < nct) & (s[k] != 0.0)) { /* Apply the transformation */ double t = 0; for (int i = k; i < _m; i++) { t += a[i][k] * a[i][j]; } t = (-t) / a[k][k]; for (int i = k; i < _m; i++) { a[i][j] += t * a[i][k]; } } /* * Place the k-th row of A into e for the * subsequent calculation of the row transformation. */ e[j] = a[k][j]; } if (k < nct) { /* * Place the transformation in U for subsequent back * multiplication. */ for (int i = k; i < _m; i++) { u[i][k] = a[i][k]; } } if (k < nrt) { // Compute the k-th row transformation and place the // k-th super-diagonal in e[k]. // Compute 2-norm without under/overflow. e[k] = 0; for (int i = k + 1; i < _n; i++) { e[k] = Fn.Hypot(e[k], e[i]); } if (e[k] != 0.0) { if (e[k + 1] < 0.0) { e[k] = -e[k]; } for (int i = k + 1; i < _n; i++) { e[i] /= e[k]; } e[k + 1] += 1.0; } e[k] = -e[k]; if ((k + 1 < _m) & (e[k] != 0.0)) { /* Apply the transformation */ for (int i = k + 1; i < _m; i++) { work[i] = 0.0; } for (int j = k + 1; j < _n; j++) { for (int i = k + 1; i < _m; i++) { work[i] += e[j] * a[i][j]; } } for (int j = k + 1; j < _n; j++) { double t = (-e[j]) / e[k + 1]; for (int i = k + 1; i < _m; i++) { a[i][j] += t * work[i]; } } } /* * Place the transformation in V for subsequent * back multiplication. */ for (int i = k + 1; i < _n; i++) { v[i][k] = e[i]; } } } /* Set up the final bidiagonal matrix or order p. */ int p = Math.Min(_n, _m + 1); if (nct < _n) { s[nct] = a[nct][nct]; } if (_m < p) { s[p - 1] = 0.0; } if (nrt + 1 < p) { e[nrt] = a[nrt][p - 1]; } e[p - 1] = 0.0; /* If required, generate U */ for (int j = nct; j < nu; j++) { for (int i = 0; i < _m; i++) { u[i][j] = 0.0; } u[j][j] = 1.0; } for (int k = nct - 1; k >= 0; k--) { if (s[k] != 0.0) { for (int j = k + 1; j < nu; j++) { double t = 0; for (int i = k; i < _m; i++) { t += u[i][k] * u[i][j]; } t = (-t) / u[k][k]; for (int i = k; i < _m; i++) { u[i][j] += t * u[i][k]; } } for (int i = k; i < _m; i++) { u[i][k] = -u[i][k]; } u[k][k] = 1.0 + u[k][k]; for (int i = 0; i < k - 1; i++) { u[i][k] = 0.0; } } else { for (int i = 0; i < _m; i++) { u[i][k] = 0.0; } u[k][k] = 1.0; } } /* If required, generate V */ for (int k = _n - 1; k >= 0; k--) { if ((k < nrt) & (e[k] != 0.0)) { for (int j = k + 1; j < nu; j++) { double t = 0; for (int i = k + 1; i < _n; i++) { t += v[i][k] * v[i][j]; } t = (-t) / v[k + 1][k]; for (int i = k + 1; i < _n; i++) { v[i][j] += t * v[i][k]; } } } for (int i = 0; i < _n; i++) { v[i][k] = 0.0; } v[k][k] = 1.0; } /* Main iteration loop for the singular values */ int pp = p - 1; int iter = 0; double eps = Number.PositiveRelativeAccuracy; while (p > 0) { int k; IterationStep step; /* Here is where a test for too many iterations would go */ /* * This section of the program inspects for * negligible elements in the s and e arrays. On * completion the variables kase and k are set as follows. * * DeflateNeglible: if s[p] and e[k-1] are negligible and k<p * SplitAtNeglible: if s[k] is negligible and k<p * QR: if e[k-1] is negligible, k<p, and s[k], ..., s[p] are not negligible. * Convergence: if e[p-1] is negligible. */ for (k = p - 2; k >= 0; k--) { if (Math.Abs(e[k]) <= eps * (Math.Abs(s[k]) + Math.Abs(s[k + 1]))) { e[k] = 0.0; break; } } if (k == p - 2) { step = IterationStep.Convergence; } else { int ks; for (ks = p - 1; ks >= k; ks--) { if (ks == k) { break; } double t = (ks != p ? Math.Abs(e[ks]) : 0.0) + (ks != k + 1 ? Math.Abs(e[ks - 1]) : 0.0); if (Math.Abs(s[ks]) <= eps * t) { s[ks] = 0.0; break; } } if (ks == k) { step = IterationStep.QR; } else if (ks == p - 1) { step = IterationStep.DeflateNeglible; } else { step = IterationStep.SplitAtNeglible; k = ks; } } k++; /* Perform the task indicated by 'step'. */ switch (step) { // Deflate negligible s(p). case IterationStep.DeflateNeglible: { double f = e[p - 2]; e[p - 2] = 0.0; for (int j = p - 2; j >= k; j--) { double t = Fn.Hypot(s[j], f); double cs = s[j] / t; double sn = f / t; s[j] = t; if (j != k) { f = (-sn) * e[j - 1]; e[j - 1] = cs * e[j - 1]; } for (int i = 0; i < _n; i++) { t = (cs * v[i][j]) + (sn * v[i][p - 1]); v[i][p - 1] = ((-sn) * v[i][j]) + (cs * v[i][p - 1]); v[i][j] = t; } } } break; // Split at negligible s(k) case IterationStep.SplitAtNeglible: { double f = e[k - 1]; e[k - 1] = 0.0; for (int j = k; j < p; j++) { double t = Fn.Hypot(s[j], f); double cs = s[j] / t; double sn = f / t; s[j] = t; f = (-sn) * e[j]; e[j] = cs * e[j]; for (int i = 0; i < _m; i++) { t = (cs * u[i][j]) + (sn * u[i][k - 1]); u[i][k - 1] = ((-sn) * u[i][j]) + (cs * u[i][k - 1]); u[i][j] = t; } } } break; // Perform one qr step. case IterationStep.QR: { /* Calculate the shift */ double scale = Math.Max(Math.Max(Math.Max(Math.Max(Math.Abs(s[p - 1]), Math.Abs(s[p - 2])), Math.Abs(e[p - 2])), Math.Abs(s[k])), Math.Abs(e[k])); double sp = s[p - 1] / scale; double spm1 = s[p - 2] / scale; double epm1 = e[p - 2] / scale; double sk = s[k] / scale; double ek = e[k] / scale; double b = (((spm1 + sp) * (spm1 - sp)) + (epm1 * epm1)) / 2.0; double c = (sp * epm1) * (sp * epm1); double shift = 0.0; if ((b != 0.0) | (c != 0.0)) { shift = Math.Sqrt((b * b) + c); if (b < 0.0) { shift = -shift; } shift = c / (b + shift); } double f = ((sk + sp) * (sk - sp)) + shift; double g = sk * ek; /* Chase zeros */ for (int j = k; j < p - 1; j++) { double t = Fn.Hypot(f, g); double cs = f / t; double sn = g / t; if (j != k) { e[j - 1] = t; } f = (cs * s[j]) + (sn * e[j]); e[j] = (cs * e[j]) - (sn * s[j]); g = sn * s[j + 1]; s[j + 1] = cs * s[j + 1]; for (int i = 0; i < _n; i++) { t = (cs * v[i][j]) + (sn * v[i][j + 1]); v[i][j + 1] = ((-sn) * v[i][j]) + (cs * v[i][j + 1]); v[i][j] = t; } t = Fn.Hypot(f, g); cs = f / t; sn = g / t; s[j] = t; f = (cs * e[j]) + (sn * s[j + 1]); s[j + 1] = ((-sn) * e[j]) + (cs * s[j + 1]); g = sn * e[j + 1]; e[j + 1] = cs * e[j + 1]; if (j < _m - 1) { for (int i = 0; i < _m; i++) { t = (cs * u[i][j]) + (sn * u[i][j + 1]); u[i][j + 1] = ((-sn) * u[i][j]) + (cs * u[i][j + 1]); u[i][j] = t; } } } e[p - 2] = f; iter = iter + 1; } break; // Convergence. case IterationStep.Convergence: { /* Make the singular values positive */ if (s[k] <= 0.0) { s[k] = (s[k] < 0.0 ? -s[k] : 0.0); for (int i = 0; i <= pp; i++) { v[i][k] = -v[i][k]; } } /* Order the singular values */ while (k < pp) { if (s[k] >= s[k + 1]) { break; } double t = s[k]; s[k] = s[k + 1]; s[k + 1] = t; if (k < _n - 1) { for (int i = 0; i < _n; i++) { t = v[i][k + 1]; v[i][k + 1] = v[i][k]; v[i][k] = t; } } if (k < _m - 1) { for (int i = 0; i < _m; i++) { t = u[i][k + 1]; u[i][k + 1] = u[i][k]; u[i][k] = t; } } k++; } iter = 0; p--; } break; } } // (vermorel) transposing the results if needed if (_transpose) { // swaping U and V double[][] temp = v; v = u; u = temp; } _u = new Matrix(u); _v = new Matrix(v); _singular = new Vector(s); InitOnDemandComputations(); }
/// <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); // We need to use transpose of H a couple of times. Matrix<double> Ht = H.Transpose(); Matrix<double> I = Matrix<double>.Build.DenseIdentity(x.RowCount, x.RowCount); Matrix<double> S = (H*P*Ht) + R; // Measurement covariance Matrix<double> K = P*Ht*S.Inverse(); // Kalman Gain P = (I - (K*H))*P; // Covariance update x = x + (K*(z - (H*x))); // State update }
/// <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) { KalmanFilter.CheckPredictParameters(F, G, Q, this); // State prediction x = F*x; // Covariance update P = (F*P*F.Transpose()) + (G*Q*G.Transpose()); }
/// <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) { KalmanFilter.CheckPredictParameters(F, Q, this); // Predict the state x = F*x; P = (F*P*F.Transpose()) + Q; }
/// <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); x = F*x; P = F*P*F.Transpose(); }