UKF для внедрения РЛС - PullRequest
Новая
       82

UKF для внедрения РЛС

1 голос
/ 02 августа 2020

Я изо всех сил пытаюсь реализовать фильтр Калмана без запаха для отслеживания объектов с помощью радара. Мой вектор состояния содержит [xyz vx vy vz], и я могу измерить [скорость rhophi theta]. Так что на первый взгляд все выглядит тривиальным, потому что оценка состояния - это просто 

  x = rho * sin(theta) * cos(phi);
  y = rho * sin(theta) * sin(phi);
  z = rho * cos(theta);
  vx = v * sin(theta) * cos(phi);
  vy = v * sin(theta) * sin(phi);
  vz = v * cos(theta);

Модель измерения также хорошо известна: 

rho = sqrt(p_x*p_x + p_y*p_y + p_z*p_z);         
phi = atan(p_y/p_x); 
theta = atan(sqrt(p_x*p_x + p_y*p_y)/p_z); 
velocity = sqrt(v_x*v_x + v_y*v_y + v_z*v_z);

Мои прогнозы основаны на модели постоянной скорости и выглядят вот так: 

    //predicted state values
    px_p = p_x + v_x*delta_t;
    py_p = p_y + v_y*delta_t;
    pz_p = p_z + v_z*delta_t;
    vx_p = v_x + err_x*delta_t;
    vy_p = v_y + err_y*delta_t;
    vz_p = v_z + err_z*delta_t;

И ... не работает. Единственный случай, когда он работает - постоянная скорость по оси x с. Может ли кто-нибудь объяснить мне, что я делаю не так? Какой должна быть Q-матрица в этом случае? Цените любые советы и подсказки. Ура, Вики.

UPD: В пакете robot_localization я обнаружил матрицу с именем transferFunction_ (), которая, как я понимаю, является функцией процесса (в справочном примере в комментариях она используется для прогнозирования сигма-точек) без шума. Он 15-мерный и реализован вот так: 

double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);

double sp = ::sin(pitch);
double cp = ::cos(pitch);
double cpi = 1.0 / cp;
double tp = sp * cpi;

double sr = ::sin(roll);
double cr = ::cos(roll);

double sy = ::sin(yaw);
double cy = ::cos(yaw);

transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;

Также есть четкая матрица processNoiseCovariance: 

processNoiseCovariance_.setZero();
processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05;
processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05;
processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06;
processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04;
processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01;
processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01;
processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01;
processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015;

1 Ответ

0 голосов
/ 31 августа 2020

Короче говоря, я не нашел решения, поэтому я просто использую два фильтра UKF на 2 плоскостях, чтобы покрыть 3D. Визуальная часть отлично работает в rviz, но я все еще реализую алгоритмы ассоциации данных, чтобы быть уверенным, что я отслеживаю одну и ту же цель с помощью обоих фильтров.

...