public void TestCalculateKalmanGain()
{
//Test that K is calculated correctly. Using an ensemble of 5 perfectly sorted ensembles for 3 states where HA equals one of the ensembles
// Two obs. obsvar 10 times bigger for one obs than the other so diff in gain should be 100 amnd hte
var M = Matrix <double> .Build;
double[,] x =
{
{ -1.0, -2.0, 0, 2, 1 },
{ -4, -2, 0, 2, 4 },
{ -40, -20, 0, 20, 40 }
};//3 states 5 members
Matrix <double> A = M.DenseOfArray(x);
double stdxxx = MathNet.Numerics.Statistics.Statistics.StandardDeviation(A.Row(1));
double[,] xx =
{
{ -4, -2, 0, 2, 4 },
{ -4, -2, 0, 2, 4 }
};
Matrix <double> HA = M.DenseOfArray(xx);
double[,] xxx =
{
{ 1.0, 0.0 },
{ 0, 100.0 }
};
Matrix <double> ObsCoVar = M.DenseOfArray(xxx);
EnsembleModels.KalmanGain KK = new KalmanGain(3, 1);
KK.CaclulateGain(A, HA, ObsCoVar);
Assert.AreEqual(0.3604, KK.K[0, 0], 0.01);
Assert.AreEqual(0.9009, KK.K[1, 0], 0.01);
Assert.AreEqual(9.0090, KK.K[2, 0], 0.01);
Assert.AreEqual(0.0036, KK.K[0, 1], 0.01);
Assert.AreEqual(0.009, KK.K[1, 1], 0.01);
Assert.AreEqual(00.0901, KK.K[2, 1], 0.01);
double[] dd = new double[HA.RowCount * HA.ColumnCount];
MathNet.Numerics.Distributions.Normal.Samples(dd, 0, 1.0);
Matrix <double> D = (M.Dense(HA.RowCount, HA.ColumnCount, dd));
D = D.TransposeThisAndMultiply(ObsCoVar.PointwiseSqrt()).Transpose();
A = A.Subtract(KK.K.Multiply(HA.Subtract(D)));
stdxxx = MathNet.Numerics.Statistics.Statistics.StandardDeviation(A.Row(1));
}
/// <summary>
/// Corrects the state error covariance based on innovation vector and Kalman update.
/// </summary>
/// <param name="innovationVector">The difference between predicted state and measurement.</param>
public void Correct(double[] innovationVector)
{
if (innovationVector is null)
{
throw new ArgumentNullException(nameof(innovationVector));
}
CheckPrerequisites();
if (innovationVector.Length != MeasurementVectorDimension)
{
throw new Exception("PredicitionError error vector (innovation vector) must have the same length as measurement.");
}
//correct state using Kalman gain
state = state.Add(KalmanGain.Dot(innovationVector));
var identity = Matrix.Identity(StateVectorDimension);
EstimateCovariance = (identity.Subtract(KalmanGain.Multiply(MeasurementMatrix))).Multiply(EstimateCovariance.Transpose());
}