Я написал реализацию фильтра Калмана с использованием Eigen Library в C ++, а также использовал реализацию по этой ссылке для проверки моего фильтра: Мой шаг прогнозирования выглядит следующим образом:
void KalmanFilter::Predict()
{
// state Estimate = state transition matrix * previous state
// No control input present.
x = A * x;
// State Covariance Matrix = (State Transition Matrix * Previous State
Covariance matrix * (State Transition Matrix)^T ) + Process Noise
P = A * P * A.transpose() + Q;
}
в то время как мой шаг обновления:
void KalmanFilter::Update(VectorXd z)
{
//Kalman Gain = (State Covariance Matrix * Measurement matrix.transpose) * (H*P*H^T + Measurement Noise)^-1
K = (P * H.transpose()) * (H * P * H.transpose()+ R).inverse();
//Estimated Stated = Estimated state + Kalman Gain (Measurement Innovation)
x = x + K*(z - H * x);
//State Covariance matrix = (Identity Matrix of the size of x.size * x.size) - K* H * P;
long x_size = x.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P = (I - K * H) * P ;
}
Мои начальные значения:
pos_x = 0.0;
pos_y = 0.0;
pos_z = 1.0;
vel_x = 10.0;
vel_y = 0.0;
vel_z = 0.0;
acc_x = 0.0;
acc_y = 0.0;
acc_z = -9.81;
, и я генерирую «поддельные данные», выполняя в цикле следующее:
double c = 0.1; // Drag resistance coefficient
double damping = 0.9 ; // Damping
double sigma_position = 0.1 ; // position_noise
// Create simulated position data
for (int i = 0; i < N; i ++)
{
acc_x = -c * pow(vel_x, 2); // calculate acceleration ( Drag Resistance)
vel_x += acc_x * dt; // Integrate acceleration to give you velocity in the x axis.
pos_x += vel_x * dt; // Integrate velocity to return the position in the x axis
acc_z = -9.806 + c * pow(vel_z, 2); // Gravitation + Drag
vel_z += acc_z * dt; // z axis velocity
pos_z += vel_z * dt; // position in z axis
// generate y position here later.
if(pos_z < 0.01)
{
vel_z = -vel_z * damping;
pos_z += vel_z * dt;
}
if (vel_x < 0.1)
{
acc_x = 0.0;
acc_z = 0.0;
}
// add some noise
pos_x = pos_x + sigma_position * process_noise(generator);
pos_y = pos_y + sigma_position * process_noise(generator);
pos_z = pos_z + sigma_position * process_noise(generator);
Затем я запускаю шаг прогнозирования и обновления:
// Prediction Step
kalmanFilter.Predict();
// Correction Step
kalmanFilter.Update(z);
, где z - это вектор 3 x 1, содержащий pos_x, pos_y and pos_z
Моя матрица перехода состояний A выглядиткак это:
A << 1, 0, 0, dt, 0, 0, dt_squared, 0 , 0,
0, 1, 0, 0, dt, 0, 0, dt_squared, 0,
0, 0, 1, 0, 0, dt, 0, 0, dt_squared,
0, 0, 0, 1, 0, 0, dt, 0, 0,
0, 0, 0, 0, 1, 0, 0 , dt, 0,
0, 0, 0, 0, 0, 1, 0, 0, dt,
0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1;
где dt_squared равно (dt * dt) /2;
P is
P<< 100, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100;
и
R << 1, 0, 0,
0, 1, 0,
0, 0, 1;
и
Q = G * G.transpose()* a * a;
гдеG - это Матрица 9 x 1
G << dt_squared, dt_squared, dt_squared, dt, dt, dt, 1, 1, 1;
a = 0.1 //( acceleration process noise)
Моя проблема в том, что моя предполагаемая позиция для y и z не совпадает с «реальными» позициями.Если вы посмотрите на следующие графики,
Вот как выглядит pos_x: 
Вот как выглядит pos_y: 
И, наконец, Z: 
Это мой первый набег с фильтрами Калмана, и я не уверен, что я делаю здесь неправильно.Моей конечной целью было бы использовать это, чтобы оценить положение дрона.Кроме того, у меня есть следующие вопросы:
В реальной ситуации, например, для дрона, как вы относитесь к выбору вашего Process Noise, если вы не можете непосредственно наблюдать за процессом?Вы просто выбираете произвольные значения?
Мои извинения за длинный пост.Любая помощь приветствуется.