我使用C ++中的Eigen Library编写了一个kalman Filter实现,并使用此link中的实现来测试我的过滤器:我的预测步骤如下所示:
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是含有pos_x, pos_y and pos_z
的3 x 1载体
我的状态转换矩阵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的估计位置偏离了“实际”位置。如果你看下面的图表,
这是我第一次尝试使用卡尔曼滤波器,我不确定我在这里做错了什么。我的最终目标是使用它来估计无人机的位置。另外,我有以下问题:
例如,对于无人机的现实生活情况,如果不能直接观察过程,您如何选择过程噪声?你只是选择任意值吗?
我为长篇大论道歉。任何帮助表示赞赏。
我不确定这是与代码相关的问题,算法实现问题还是期望问题。
您确实意识到,如果虚假数据中存在过多的操作,像这样的过滤器将不会重现真实数据,甚至不会重现真实数据。
此外,您的图表不存在。
我知道我的回答不符合社区标准,但我无法发表评论或者我会这样做。
在您提供图表并根据更新速率检查路径曲率之前,我不会尝试详细说明。此外,过滤器需要“调整”到特定系统。您可能需要使用噪声参数来更好地调整它。对于机动轨道,可能需要使用更高阶滤波器,Singer或Jerk滤波器......滤波器需要对系统建模得足够好。根据您的更新矩阵,您似乎有一个抛物线(二阶)估计。您可能还想在其他非s / w或代码特定的论坛中询问此问题。
每个系统都有差异。假设过滤器的方差为+ -1%,实际值为+ -5%;如果您预测值,则必须选择更新以使用预测值或测量值。取决于你更相信哪一个。否则,您的过滤器始终基于它自己的值开发...