Библиотека фильтров Калмана C ++, выдающая 1. # R (NaN) результаты
В настоящее время я пытаюсь использовать Бесплатная C ++ расширенная библиотека фильтров Калмана . Я понимаю основы фильтра Калмана, однако у меня есть проблема значений NaN, создаваемых с помощью этой библиотеки. Кто-нибудь на SO имеет опыт использования алгоритма фильтра Калмана, чтобы обнаружить мою ошибку?
Это мой фильтр:
class PointEKF : public Kalman::EKFilter<double,1,false,true,false> {
public:
PointEKF() : Period(0.0) {
setDim(3, 1, 3, 1, 1);
}
void SetPeriod(double p) {
Period = p;
}
protected:
void makeBaseA() {
A(1, 1) = 1.0;
//A(1, 2) = Period;
//A(1, 3) = Period*Period / 2;
A(2, 1) = 0.0;
A(2, 2) = 1.0;
//A(2, 3) = Period;
A(3, 1) = 0.0;
A(3, 2) = 0.0;
A(3, 3) = 1.0;
}
void makeBaseH() {
H(1, 1) = 1.0;
H(1, 2) = 0.0;
H(1, 3) = 0.0;
}
void makeBaseV() {
V(1, 1) = 1.0;
}
void makeBaseW() {
W(1, 1) = 1.0;
W(1, 2) = 0.0;
W(1, 3) = 0.0;
W(2, 1) = 0.0;
W(2, 2) = 1.0;
W(2, 3) = 0.0;
W(3, 1) = 0.0;
W(3, 2) = 0.0;
W(3, 3) = 1.0;
}
void makeA() {
double T = Period;
A(1, 1) = 1.0;
A(1, 2) = T;
A(1, 3) = (T*T) / 2;
A(2, 1) = 0.0;
A(2, 2) = 1.0;
A(3, 3) = T;
A(3, 1) = 0.0;
A(3, 2) = 0.0;
A(3, 3) = 1.0;
}
void makeH() {
double T = Period;
H(1, 1) = 1.0;
H(1, 2) = T;
H(1, 3) = T*T / 2;
}
void makeProcess() {
double T = u(1);
Vector x_(x.size());
x_(1) = x(1) + x(2) * T + (x(3) * T*T / 2);
x_(2) = x(2) + x(3) * T;
x_(3) = x(3);
x.swap(x_);
}
void makeMeasure() {
z(1) = x(1);
}
double Period;
};
Я использовал это следующим образом:
void init() {
int n = 3;
static const double _P0[] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
};
Matrix P0(n, n, _P0);
Vector x(3);
x(1) = getPoint(0);
x(2) = getVelocity(0);
x(3) = getAccleration(0);
filterX.init(x, P0);
}
а также,
Vector measurement(1), input(1), u(1);
u(1) = 0.400;
double start = data2->positionTimeCounter;
double end = data->positionTimeCounter;
double period = (end - start) / (1000*1000);
filterX.SetPeriod(period);
measurement(1) = getPoint(0);
input(1) = period;
filterX.step(input, measurement);
auto x = filterX.predict(u);
Замечания:
Данные, которые я использую, представляют собой x точек, сгенерированных из единичного круга.
Решение
Если вы используете базовые версии матриц:
A = [ 1 0 0;
0 1 0;
0 0 1 ];
H = [ 1 0 0 ];
у вас нет наблюдаемой системы, потому что ваши измерения фиксируют только первое состояние (положение), и в матрице А нет связи между положением и его производными (скорость, ускорение). матрица наблюдаемости как следует:
O = [ H;
H*A;
H*A*A ];
O = [ 1 0 0;
1 0 0;
1 0 0 ];
что, очевидно, является единичным, то есть ваша система не наблюдаема. И подача этого с помощью алгоритма EKF должна привести к ошибке (ситуация должна быть обнаружена алгоритмом), но если она не будет обнаружена, это приведет к результатам NaN в оценках, точно так же, как вы испытываете.
Теперь матрица A из функции makeA () больше подходит:
A = [ 1 h h*h/2;
0 1 h;
0 0 1 ];
H = [ 1 0 0 ]; // use this H matrix (not [ 1 h h*h/2 ])
приводя к матрице наблюдаемости:
O = [ 1 0 0;
1 h h*h/2;
1 2*h 2*h*h ];
который является полным рангом (не единственным), и, таким образом, у вас есть наблюдаемая система.
Алгоритм фильтрации Калмана может быть весьма чувствительным к кондиционирование матриц, Это означает, что если временной шаг действительно мал (например, 1e-6), вам нужно использовать непрерывную версию. Кроме того, проблема NaN может исходить от линейного решателя (решает линейную систему уравнений), который необходим в алгоритме KF. Если бы автор библиотеки использовал наивный метод (например, устранение Гаусса, LU-разложение с или без стержней, Холецкий без стержней и т. Д.), То это значительно усложнило бы проблему численного кондиционирования.
Нотабене Вы должны начать фильтрацию KF с очень высокой P-матрицы, потому что начальный P должен отражать неопределенность вашего начального вектора состояния, которая обычно очень высока, поэтому P должно быть около 1000 * identity,
Другие решения
Других решений пока нет …