// KalmanInitialized is called for initializing the initial estimate public void KalmanInitialized(Matrixd stateTransMatrix, Matrixd controlMatrix, Matrixd inputVar, Matrixd procNoiseVector, Matrixd obsvMatrix, Matrixd measNoiseVector) { // assign the variables if (stateTransMatrix.rows != Fd.rows || stateTransMatrix.columns != Fd.columns) { throw new ArgumentException("Can not assign the state transition matrix with different rows or columns.", nameof(stateTransMatrix)); } else { Fd = stateTransMatrix; } if (controlMatrix.rows != Gd.rows || controlMatrix.columns != Gd.columns) { throw new ArgumentException("Can not assign the control matrix with different rows or columns.", nameof(controlMatrix)); } else { Gd = controlMatrix; } if (inputVar.rows != ud.rows || inputVar.columns != ud.columns) { throw new ArgumentException("Can not assign the input variable with different rows or columns.", nameof(inputVar)); } else { ud = inputVar; } if (procNoiseVector.rows != wd.rows || procNoiseVector.columns != wd.columns) { throw new ArgumentException("Can not assign the process noise vector with different rows or columns.", nameof(procNoiseVector)); } else { wd = procNoiseVector; } if (obsvMatrix.rows != Hd.rows || obsvMatrix.columns != Hd.columns) { throw new ArgumentException("Can not assign the observation matrix with different rows or columns.", nameof(obsvMatrix)); } else { Hd = obsvMatrix; } if (measNoiseVector.rows != vd.rows || measNoiseVector.columns != vd.columns) { throw new ArgumentException("Can not assign the measurement noise vector with different rows or columns.", nameof(measNoiseVector)); } else { vd = measNoiseVector; } // construct the process noise uncertainty and measurement noise uncertainty Qd = Matrixd.Mul(wd, Matrixd.Transpose(wd)); Rd = Matrixd.Mul(vd, Matrixd.Transpose(vd)); // calculate the initial estimate(initial state vector and estimate uncertainty) for updating iteration KalmanPredictd(); }
// KalmanCorrectd is called for correcting the predict of the pre-state(first update for every iteration) public void KalmanCorrectd(Matrixd measurement) { // update the output vector(measurement result) if (measurement.rows != zd.rows || measurement.columns != zd.columns) { throw new ArgumentException("Can not update the measurement with different rows or columns.", nameof(measurement)); } else { zd = measurement; } // compute the kalman gain and correct the discrete state and estimate uncertainty Kd = Matrixd.Mul(Matrixd.Mul(Pd, Matrixd.Transpose(Hd)), Matrixd.Inverse(Matrixd.Mul(Matrixd.Mul(Hd, Pd), Matrixd.Transpose(Hd)) + Rd)); xd = xd + Matrixd.Mul(Kd, (zd - Matrixd.Mul(Hd, xd))); Pd = Matrixd.Mul(Matrixd.Mul((Matrixd.Identity(xd.rows, xd.rows) - Matrixd.Mul(Kd, Hd)), Pd), Matrixd.Transpose(Matrixd.Identity(xd.rows, xd.rows) - Matrixd.Mul(Kd, Hd))) + Matrixd.Mul(Matrixd.Mul(Kd, Rd), Matrixd.Transpose(Kd)); }
// KalmanPredictd is called for predicting the state(second update for every iteration) public void KalmanPredictd() { // predict the discrete state and covariance xd = Matrixd.Mul(Fd, xd) + Matrixd.Mul(Gd, ud); Pd = Matrixd.Mul(Matrixd.Mul(Fd, Pd), Matrixd.Inverse(Fd)) + Qd; }