Интерпретация и корректировка прогноза скорости фильтра Калмана - PullRequest
Новая
       154

Интерпретация и корректировка прогноза скорости фильтра Калмана

2 голосов
/ 10 июля 2020

Я использую MATH. NET реализацию фильтра Калмана

Мой пример похож на пример, описанный в этом очень полезном руководстве :

Представьте грузовик, движущийся по прямым рельсам без трения. Первоначально грузовик неподвижен в позиции 0, но на него в разных направлениях действуют случайные неконтролируемые силы. Мы измеряем положение грузовика каждые Δt секунд, но эти измерения неточны; мы хотим сохранить модель положения и скорости грузовика.

Положение и скорость грузовика описываются линейным пространством состояний

enter image description here where x' is the velocity, that is, the derivative of position with respect to time.

Math details can be found by the mentioned link.

Finally, I have this very simple .NET implementation with 2 filter parameters (_measurementCovariance and _plantNoiseVar):

enter image description here enter image description here enter image description here

//set up filter
var xState = Matrix.Build.Dense(2, 1, new[] { x0, v0 });

var measurementCovarianceMatrix = Matrix.Build.Dense(2, 2, 
new[] { _measurementCovariance, _measurementCovariance / dt,
       _measurementCovariance / dt, 2 * _measurementCovariance / (dt * dt) });

_dkf = new DiscreteKalmanFilter(xState, measurementCovarianceMatrix); 

_stateTransitionMatrixF = Matrix.Build.Dense(2, 2, new[] { 1d, 0d, dt, 1 });
_plantNoiseMatrixG = Matrix.Build.Dense(2, 1, new[] { (dt * dt) / 2, dt});
_plantNoiseVarianceQ = (_plantNoiseMatrixG.Transpose() * _plantNoiseMatrixG) * _plantNoiseVar; 
_measurementVarianceMatrixR = Matrix.Build.Dense(1, 1, new[] { _measurementCovariance  });
_measurementMatrixH = Matrix.Build.Dense(1, 2, new[] { 1d, 0d });


//update function
private void Update(double newPosition, double dt)
{
   var z = Matrix.Build.Dense(1, 1, new[] { newPosition });
   _stateTransitionMatrixF[0, 1] = dt;
   _plantNoiseMatrixG[0, 0] = (dt* dt) / 2;
   _plantNoiseMatrixG[1, 0] = dt;
 
   _dkf.Predict(_stateTransitionMatrixF, _plantNoiseMatrixG, _plantNoiseVarianceQ);
   _dkf.Update(z, _measurementMatrixH, _measurementVarianceMatrixR);
}

Then I tested this implementation for real data I have:

For each X position I invoked Update function and stored Kalman Velocity prediction via:

kalmanPosition = _dkf.State[0, 0];
kalmanVelocity = _dkf.State[1, 0];
kalmanSpeedInstant = (kalmanPosition - _prevPosition) / dt;
_prevPosition = kalmanPosition;

Here is a plot with results:

введите описание изображения здесь

Чего я не могу понять, почему для моего примера так мала калмановская скорость. Может ли кто-нибудь помочь мне с этим примером.

  1. Правильно ли я интерпретирую результаты?
  2. Это _dkf.State [1, 0] скорость или я ошибаюсь?
  3. Были ли ошибки в моих предположениях?
  4. Как я могу получить более точную скорость?

I использовал параметры:

_measurementCovariance = 1.5 и _plantNoiseVar = 3.0.

Данные, которые я использовал:

1 Ответ

0 голосов
/ 14 июля 2020

Мой опыт работы с фильтрами Калмана был длинным, go и вытянутым, но я помню, что если фильтр уверен, что у него уже есть правильный ответ, он в значительной степени игнорирует новые наблюдения. Проверьте, что произошло с матрицей дисперсии и ковариации в точке, где фильтр перестает отслеживать.

...