/**
* static extended Kalman filter with noise on the horizontal part of the process noise matrix
*/
public PedrestianStaticExtendedKalmanFilter()
{
/** vector for the predicted state
*/
x_pred = new SimpleMatrix <Matrix>(numStates, 1);
/** state transition matrix F
*/
F = SimpleMatrix <Matrix> .identity(numStates);
/** process noise matrix Q
*/
Q = new SimpleMatrix <Matrix>(numStates, numStates);
/** variance-covariance matrix of the state vector
*/
P_pred = new SimpleMatrix <Matrix>(numStates, numStates);
/** measurement vector with numStates entries
*/
x_meas = new SimpleMatrix <Matrix>(numStates, 1);
/** variance-covariance matrix of the measurement vector
*/
P_meas = new SimpleMatrix <Matrix>(numStates, numStates);
// Initialization of the state transition matrix
F.set(idxClockBias, idxClockDrift, DELTA_T);
// Initialization of the process noise matrix
initQ();
}
public StaticExtendedKalmanFilter()
{
x_pred = new SimpleMatrix <Matrix>(numStates, 1);
F = SimpleMatrix <Matrix> .identity(numStates);
Q = new SimpleMatrix <Matrix>(numStates, numStates);
P_pred = new SimpleMatrix <Matrix>(numStates, numStates);
x_meas = new SimpleMatrix <Matrix>(numStates, 1);
P_meas = new SimpleMatrix <Matrix>(numStates, numStates);
S_f = h_0 / 2.0;
S_g = 2.0 * Math.Pow(Constants.PI_ORBIT, 2) * h_2;
// Initialization of the state transition matrix
F.set(idxClockBias, idxClockDrift, DELTA_T);
// Initialization of the process noise matrix
initQ();
}
public DynamicExtendedKalmanFilter()
{
x_pred = new SimpleMatrix <Matrix>(numStates, 1);
F = SimpleMatrix <Matrix> .identity(numStates);
Q = new SimpleMatrix <Matrix>(numStates, numStates);
P_pred = new SimpleMatrix <Matrix>(numStates, numStates);
x_meas = new SimpleMatrix <Matrix>(numStates, 1);
P_meas = new SimpleMatrix <Matrix>(numStates, numStates);
// Initialization of the state transition matrix
F.set(idxX, idxU, DELTA_T);
F.set(idxY, idxV, DELTA_T);
F.set(idxZ, idxW, DELTA_T);
F.set(idxClockBias, idxClockDrift, DELTA_T);
// Initialization of the process noise matrix
initQ();
}
/**
* Returns the Q matrix.
*/
public SimpleMatrix <DMatrixRMaj> getQ()
{
SimpleMatrix <DMatrixRMaj> Q = SimpleMatrix <DMatrixRMaj> .identity(QR.numRows());
int N = Math.Min(QR.numCols(), QR.numRows());
// compute Q by first extracting the householder vectors from the columns of QR and then applying it to Q
for (int j = N - 1; j >= 0; j--)
{
SimpleMatrix <DMatrixRMaj> u = new SimpleMatrix <DMatrixRMaj>(QR.numRows(), 1);
u.insertIntoThis(j, 0, QR.extractMatrix(j, SimpleMatrix <DMatrixRMaj> .END, j, j + 1));
u.set(j, 1.0);
// A = (I - γ*u*u<sup>T</sup>)*A<br>
Q = Q.plus(-gammas[j], u.mult(u.transpose()).mult(Q)) as SimpleMatrix <DMatrixRMaj>;
}
return(Q);
}
public override Coordinates <Matrix> calculatePose(Constellation constellation)
{
/** number of satellites in constellation
*/
int CONSTELLATION_SIZE = constellation.getUsedConstellationSize();
/** innovation sequence vector
*/
SimpleMatrix <Matrix> gamma;
// Initialize a variable to hold the predicted (geometric) distance towards each satellite
/** geometric distance in meters to every satellite. internal variable.
*/
double distPred = 0.0; // geometric Distance
/** counter for the satellites used in the position computation
*/
int usedInCalculations = 0;
/**
* approximate position of the receiver in ECEF.
*/
SimpleMatrix <Matrix> rxPosSimpleVector = Constellation.getRxPosAsVector(constellation.getRxPos());
if (firstExecution)
{
// Initialize the state vector
x_pred.set(idxX, rxPosSimpleVector.get(0));
x_pred.set(idxY, rxPosSimpleVector.get(1));
x_pred.set(idxZ, rxPosSimpleVector.get(2));
x_pred.set(idxClockBias, INITIAL_CLOCK_BIAS);
x_pred.set(idxClockDrift, 0.0); // clock bias drift
// Initialize the VCM of the state vector
P_pred.set(idxX, idxX, Math.Pow(INITIAL_SIGMAPOS, 2));
P_pred.set(idxY, idxY, Math.Pow(INITIAL_SIGMAPOS, 2));
P_pred.set(idxZ, idxZ, Math.Pow(INITIAL_SIGMAPOS, 2));
P_pred.set(idxClockBias, idxClockBias, Math.Pow(INITIAL_SIGMACLOCKBIAS, 2));
P_pred.set(idxClockDrift, idxClockDrift, Math.Pow(INITIAL_SIGMACLOCKDRIFT, 2));
x_pred.set(F.mult(x_pred));
P_pred = F.mult(P_pred.mult(F.transpose())).plus(Q);
}
else
{
// Perform time-prediction of the state vector and its VCM
x_pred.set(F.mult(x_meas));
P_pred = F.mult(P_meas.mult(F.transpose())).plus(Q);
}
/** Kalman gain matrix K
*/
SimpleMatrix <Matrix> K;
/** Innovation covariance
*/
SimpleMatrix <Matrix> S;
// Initialize the variables related to the measurement model
/** Observation Matrix H
*/
SimpleMatrix <Matrix> H = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, numStates);
/** pseudorange vector, one entry for every used satellite
*/
SimpleMatrix <Matrix> prVect = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // pseurorange
/** predicted pseudoranges vector, one entry for every used satellite
*/
SimpleMatrix <Matrix> measPred = new SimpleMatrix <Matrix>(CONSTELLATION_SIZE, 1); // pseudor. predicted
/** variance-covariance matrix of the measurements R
*/
SimpleMatrix <Matrix> R = SimpleMatrix <Matrix> .identity(CONSTELLATION_SIZE);
// R = R.divide(1.0/100.0);
// meas variance of each satellite
// SimpleMatrix<Matrix> sigma2C1 = new SimpleMatrix<Matrix>(CONSTELLATION_SIZE, 1);
double sigma2Meas = Math.Pow(5, 2);
// Form the observation matrix H
for (int k = 0; k < CONSTELLATION_SIZE; k++)
{
if (constellation.getSatellite(k).getSatellitePosition() == null)
{
continue;
}
// Get the raw pseudoranges for each satellite
prVect.set(k, constellation.getSatellite(k).getPseudorange());
// Compute the predicted (geometric) distance towards each satellite
distPred = Math.Sqrt(
Math.Pow(constellation.getSatellite(k).getSatellitePosition().getX()
- x_pred.get(idxX), 2)
+ Math.Pow(constellation.getSatellite(k).getSatellitePosition().getY()
- x_pred.get(idxY), 2)
+ Math.Pow(constellation.getSatellite(k).getSatellitePosition().getZ()
- x_pred.get(idxZ), 2)
);
// Set the values inside the H matrix
H.set(k, idxX, (x_pred.get(idxX) - constellation.getSatellite(k).getSatellitePosition().getX()) / distPred);
H.set(k, idxY, (x_pred.get(idxY) - constellation.getSatellite(k).getSatellitePosition().getY()) / distPred);
H.set(k, idxZ, (x_pred.get(idxZ) - constellation.getSatellite(k).getSatellitePosition().getZ()) / distPred);
H.set(k, idxClockBias, 1.0);
// Form the predicted measurement towards each satellite
measPred.set(k, distPred + x_pred.get(idxClockBias)
- constellation.getSatellite(k).getClockBias()
+ constellation.getSatellite(k).getAccumulatedCorrection());
// Form the VCM of the measurements (R)
elev = constellation.getSatellite(k).getRxTopo().getElevation() * (Math.PI / 180.0);
R.set(k, k, sigma2Meas * Math.Pow(a + b * Math.Exp(-elev / 10.0), 2));
usedInCalculations++;
}
if (usedInCalculations > 0)
{
// Compute the Kalman Gain. Catch exception if matrix inversion fails.
try
{
K = P_pred.mult(H.transpose().mult((H.mult(P_pred.mult(H.transpose())).plus(R)).invert()));
S = H.mult(P_pred.mult(H.transpose())).plus(R);
}
catch (Exception e)
{
Log.Error(NAME, new String(" Matrix inversion failed"), e);
return(Coordinates <Matrix> .globalXYZInstance(
rxPosSimpleVector.get(0),
rxPosSimpleVector.get(1),
rxPosSimpleVector.get(2)));
}
// Compute the Kalman innovation sequence
gamma = prVect.minus(measPred);
// Perform the measurement update
x_meas = x_pred.plus(K.mult(gamma));
P_meas = (SimpleMatrix <Matrix> .identity(numStates).minus((K.mult(H)))).mult(P_pred);
//// x_meas and P_meas are being used for the next set of measurements
//if (kalmanParamLogger.isStarted())
// kalmanParamLogger.logKalmanParam(x_meas, P_meas, numStates, gamma, S, CONSTELLATION_SIZE, constellation);
firstExecution = false;
return(Coordinates <Matrix> .globalXYZInstance(
x_meas.get(idxX),
x_meas.get(idxY),
x_meas.get(idxZ)));
}
else
{
return(Coordinates <Matrix> .globalXYZInstance(
rxPosSimpleVector.get(0),
rxPosSimpleVector.get(1),
rxPosSimpleVector.get(2)));
}
}
