Пример #1
0
        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));
        }
Пример #2
0
        /// <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());
        }