예제 #1
0
        public void Update(Vector <double> measurement, MeasurementFunc measurementFunction, MeasurementJacobianFunc measurementFunctionJacobian, Matrix <double> noise)
        {
            var jacobian   = measurementFunctionJacobian(State);
            var kalmanGain = Uncertainty * jacobian.Transpose() * (jacobian * Uncertainty * jacobian.Transpose() + noise).Inverse();

            State       = State + kalmanGain * (measurement - measurementFunction(State));
            Uncertainty = (DenseMatrix.CreateIdentity(State.Count) - kalmanGain * jacobian) * Uncertainty;
        }
예제 #2
0
        public void Update(Vector <double> measurement, MeasurementFunc measurementFunction, MeasurementJacobianFunc measurementFunctionJacobian, Matrix <double> noise)
        {
            var jacobian   = measurementFunctionJacobian(Mean);
            var kalmanGain = Covariance * jacobian.Transpose() * (jacobian * Covariance * jacobian.Transpose() + noise).Inverse();

            Mean       = Mean + kalmanGain * (measurement - measurementFunction(Mean));
            Covariance = (DenseMatrix.Identity(Mean.Count) - kalmanGain * jacobian) * Covariance;
        }