经过卡尔曼滤波的 GPS 数据仍然波动很大
Kalman-filtered GPS data is still fluctuating a lot
大家好！
我正在编写一个 Android 应用程序，它使用设备 GPS 来计算车辆的行驶速度。这应该精确到大约 1-2 km/h，我通过查看两个 GPS 位置之间的距离并将其除以这些位置分开的时间来做到这一点，非常简单，然后这样做对于最后三个记录的坐标，并将其平衡。
我在后台服务中获取 GPS 数据，它有自己的循环程序的处理程序，所以每当我从 LocationListener 获取新位置时，我都会调用 Kalmans update() 方法并在中调用 predict()通过在 predict()
之后调用 sendEmptyDelayedMessage 来定期处理程序
我已阅读 Smooth GPS data 并且实际上还尝试在 villoren 对该主题的回答中提供的 github 中实施过滤器，这也产生了波动的结果。
然后我改编了本教程 http://www.codeproject.com/Articles/326657/KalmanDemo 中的演示代码，我现在正在使用它。我手工完成了所有数学运算以更好地理解过滤器，我不确定我是否完全理解他提供的源代码，但这就是我现在正在使用的：
我注释掉的部分
/*// K = P * H^T *S^-1
double k = m_p0 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k;
m_x1 += y1 * k;
// P = (I – K * H) * P
m_p0 = m_p0 - k* m_p0;
m_p1 = m_p1 - k* m_p1;
m_p2 = m_p2 - k* m_p2;
m_p3 = m_p3 - k* m_p3;
*/
我不同意所提供代码的数学运算，但考虑到（他说）他已经在火箭制导系统中实现了卡尔曼滤波器，我倾向于相信他的数学运算是正确的；）
public class KalmanFilter {
/*
X = State
F = rolls X forward, typically be some time delta.
U = adds in values per unit time dt.
P = Covariance – how each thing varies compared to each other.
Y = Residual (delta of measured and last state).
M = Measurement
S = Residual of covariance.
R = Minimal innovative covariance, keeps filter from locking in to a solution.
K = Kalman gain
Q = minimal update covariance of P, keeps P from getting too small.
H = Rolls actual to predicted.
I = identity matrix.
*/
//State X[0] =position, X[1] = velocity.
private double m_x0, m_x1;
//P = a 2x2 matrix, uncertainty
private double m_p0, m_p1,m_p2, m_p3;
//Q = minimal covariance (2x2).
private double m_q0, m_q1, m_q2, m_q3;
//R = single value.
private double m_r;
//H = [1, 0], we measure only position so there is no update of state.
private final double m_h1 = 1, m_h2 = 0;
//F = 2x2 matrix: [1, dt], [0, 1].
public void update(double m, double dt){
// Predict to now, then update.
// Predict:
// X = F*X + H*U
// P = F*X*F^T + Q.
// Update:
// Y = M – H*X Called the innovation = measurement – state transformed by H.
// S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R
// K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance.
// X = X + K*Y Update with gain the new measurement
// P = (I – K * H) * P Update covariance to this time.
// X = F*X + H*U
double oldX = m_x0;
m_x0 = m_x0 + (dt * m_x1);
// P = F*X*F^T + Q
m_p0 = m_p0 + dt * (m_p2 + m_p1) + dt * dt * m_p3 + m_q0;
m_p1 = m_p1 + dt * m_p3 + m_q1;
m_p2 = m_p2 + dt * m_p3 + m_q2;
m_p3 = m_p3 + m_q3;
// Y = M – H*X
//To get the change in velocity, we pretend to be measuring velocity as well and
//use H as [1,1]
double y0 = m - m_x0;
double y1 = ((m - oldX) / dt) - m_x1;
// S = H*P*H^T + R
//because H is [1,0], s is only a single value
double s = m_p0 + m_r;
/*// K = P * H^T *S^-1
double k = m_p0 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k;
m_x1 += y1 * k;
// P = (I – K * H) * P
m_p0 = m_p0 - k* m_p0;
m_p1 = m_p1 - k* m_p1;
m_p2 = m_p2 - k* m_p2;
m_p3 = m_p3 - k* m_p3;
*/
// K = P * H^T *S^-1
double k0 = m_p0 / s;
double k1 = m_p2 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k0;
m_x1 += y1 * k1;
// P = (I – K * H) * P
m_p0 = m_p0 - k0* m_p0;
m_p1 = m_p1 - k0* m_p1;
m_p2 = m_p2 - k1* m_p2;
m_p3 = m_p3 - k1* m_p3;
}
public void predict(double dt){
//X = F * X + H * U Rolls state (X) forward to new time.
m_x0 = m_x0 + (dt * m_x1);
//P = F * P * F^T + Q Rolls the uncertainty forward in time.
m_p0 = m_p0 + dt * (m_p2 + m_p1) + dt * dt * m_p3 + m_q0;
/* m_p1 = m_p1+ dt * m_p3 + m_q1;
m_p2 = m_p2 + dt * m_p3 + m_q2;
m_p3 = m_p3 + m_q3;*/
}
/// <summary>
/// Reset the filter.
/// </summary>
/// <param name="qx">Measurement to position state minimal variance.</param>
/// <param name="qv">Measurement to velocity state minimal variance.</param>
/// <param name="r">Measurement covariance (sets minimal gain).</param>
/// <param name="pd">Initial variance.</param>
/// <param name="ix">Initial position.</param>
/**
*
* @param qx Measurement to position state minimal variance = accuracy of gps
* @param qv Measurement to velocity state minimal variance = accuracy of gps
* @param r Masurement covariance (sets minimal gain) = 0.accuracy
* @param pd Initial variance = accuracy of gps data 0.accuracy
* @param ix Initial position = position
*/
public void reset(double qx, double qv, double r, double pd, double ix){
m_q0 = qx; m_q1 = qv;
m_r = r;
m_p0 = m_p3 = pd;
m_p1 = m_p2 = 0;
m_x0 = ix;
m_x1 = 0;
}
public double getPosition(){
return m_x0;
}
public double getSpeed(){
return m_x1;
}
}
我正在使用两个一维过滤器，一个用于纬度，一个用于经度，然后在每次预测调用后从中构建一个新的位置对象。
我的初始化是qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy/10, pd = gpsAccuracy/10, ix = initial position.
我在从教程中获取代码后使用了这些值，这是他在评论中推荐的。
使用这个，我得到的速度是 a) 波动很大，b) 速度很慢，我得到的速度从 50 到几百 km/h 走路时，然后偶尔5-7，这更准确，但我需要速度保持一致，至少在一个合理的范围内。
试试这个简单的改变：
float speed = location.getSpeed() x 4;
GPS 接收器提供的 GPS 位置已经经过严格的 Kalman 滤波。如果位置仍在跳跃，则无法使用卡尔曼滤波器很好地解决该问题。
原因是低速移动不能很好地给出稳定的位置和速度（和方向）
只需删除所有低于 10km/h 的位置，将不再需要任何过滤。
我看到了几个问题：
- 您的
update() 包含预测 和 更新，但您也有一个 predict()，因此如果您实际调用 predict()（你没有包括外循环）。
- 对于您的测量值是位置还是位置和速度存在一些混淆。您可以看到评论声称
H=[1,0] 和 H=[1,1]（他们可能表示 H=[1,0;0,1]）因为矩阵数学是手写的，所以关于单一测量的假设被烘焙到所有矩阵中步骤，但代码仍会尝试 "measure" 速度。
- 对于从位置估计速度的 KF，您不想像那样注入合成速度（作为一阶差分）。让那个结果从钦哲基金会自然发生。对于
H=[1,0]，您可以看到 K=PH'/S 应该有 2 行，并且都适用于 y0。这将同时更新 x0 和 x1.
除了看看他们用 H 做了什么，我并没有真正检查矩阵数学。你真的应该用一个很好的矩阵库来开发这种算法（例如 numpy，用于 Python，或用于 C++ 的 Eigen）。当您进行微不足道的更改时（例如，如果您想尝试使用 2D 过滤器），这将为您节省大量代码更改，并避免简单的矩阵数学错误会让您抓狂。如果您必须针对完全手写的矩阵运算进行优化，请最后进行优化，以便比较您的结果并验证您的手写编码。
最后，关于您的具体应用，其他帖子完全正确：GPS 已经在过滤数据，输出之一是速度。
大家好！
我正在编写一个 Android 应用程序，它使用设备 GPS 来计算车辆的行驶速度。这应该精确到大约 1-2 km/h，我通过查看两个 GPS 位置之间的距离并将其除以这些位置分开的时间来做到这一点，非常简单，然后这样做对于最后三个记录的坐标，并将其平衡。
我在后台服务中获取 GPS 数据，它有自己的循环程序的处理程序，所以每当我从 LocationListener 获取新位置时，我都会调用 Kalmans update() 方法并在中调用 predict()通过在 predict()
之后调用 sendEmptyDelayedMessage 来定期处理程序我已阅读 Smooth GPS data 并且实际上还尝试在 villoren 对该主题的回答中提供的 github 中实施过滤器，这也产生了波动的结果。 然后我改编了本教程 http://www.codeproject.com/Articles/326657/KalmanDemo 中的演示代码，我现在正在使用它。我手工完成了所有数学运算以更好地理解过滤器，我不确定我是否完全理解他提供的源代码，但这就是我现在正在使用的：
我注释掉的部分
/*// K = P * H^T *S^-1
double k = m_p0 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k;
m_x1 += y1 * k;
// P = (I – K * H) * P
m_p0 = m_p0 - k* m_p0;
m_p1 = m_p1 - k* m_p1;
m_p2 = m_p2 - k* m_p2;
m_p3 = m_p3 - k* m_p3;
*/
我不同意所提供代码的数学运算，但考虑到（他说）他已经在火箭制导系统中实现了卡尔曼滤波器，我倾向于相信他的数学运算是正确的；）
public class KalmanFilter {
/*
X = State
F = rolls X forward, typically be some time delta.
U = adds in values per unit time dt.
P = Covariance – how each thing varies compared to each other.
Y = Residual (delta of measured and last state).
M = Measurement
S = Residual of covariance.
R = Minimal innovative covariance, keeps filter from locking in to a solution.
K = Kalman gain
Q = minimal update covariance of P, keeps P from getting too small.
H = Rolls actual to predicted.
I = identity matrix.
*/
//State X[0] =position, X[1] = velocity.
private double m_x0, m_x1;
//P = a 2x2 matrix, uncertainty
private double m_p0, m_p1,m_p2, m_p3;
//Q = minimal covariance (2x2).
private double m_q0, m_q1, m_q2, m_q3;
//R = single value.
private double m_r;
//H = [1, 0], we measure only position so there is no update of state.
private final double m_h1 = 1, m_h2 = 0;
//F = 2x2 matrix: [1, dt], [0, 1].
public void update(double m, double dt){
// Predict to now, then update.
// Predict:
// X = F*X + H*U
// P = F*X*F^T + Q.
// Update:
// Y = M – H*X Called the innovation = measurement – state transformed by H.
// S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R
// K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance.
// X = X + K*Y Update with gain the new measurement
// P = (I – K * H) * P Update covariance to this time.
// X = F*X + H*U
double oldX = m_x0;
m_x0 = m_x0 + (dt * m_x1);
// P = F*X*F^T + Q
m_p0 = m_p0 + dt * (m_p2 + m_p1) + dt * dt * m_p3 + m_q0;
m_p1 = m_p1 + dt * m_p3 + m_q1;
m_p2 = m_p2 + dt * m_p3 + m_q2;
m_p3 = m_p3 + m_q3;
// Y = M – H*X
//To get the change in velocity, we pretend to be measuring velocity as well and
//use H as [1,1]
double y0 = m - m_x0;
double y1 = ((m - oldX) / dt) - m_x1;
// S = H*P*H^T + R
//because H is [1,0], s is only a single value
double s = m_p0 + m_r;
/*// K = P * H^T *S^-1
double k = m_p0 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k;
m_x1 += y1 * k;
// P = (I – K * H) * P
m_p0 = m_p0 - k* m_p0;
m_p1 = m_p1 - k* m_p1;
m_p2 = m_p2 - k* m_p2;
m_p3 = m_p3 - k* m_p3;
*/
// K = P * H^T *S^-1
double k0 = m_p0 / s;
double k1 = m_p2 / s;
// double LastGain = k;
// X = X + K*Y
m_x0 += y0 * k0;
m_x1 += y1 * k1;
// P = (I – K * H) * P
m_p0 = m_p0 - k0* m_p0;
m_p1 = m_p1 - k0* m_p1;
m_p2 = m_p2 - k1* m_p2;
m_p3 = m_p3 - k1* m_p3;
}
public void predict(double dt){
//X = F * X + H * U Rolls state (X) forward to new time.
m_x0 = m_x0 + (dt * m_x1);
//P = F * P * F^T + Q Rolls the uncertainty forward in time.
m_p0 = m_p0 + dt * (m_p2 + m_p1) + dt * dt * m_p3 + m_q0;
/* m_p1 = m_p1+ dt * m_p3 + m_q1;
m_p2 = m_p2 + dt * m_p3 + m_q2;
m_p3 = m_p3 + m_q3;*/
}
/// <summary>
/// Reset the filter.
/// </summary>
/// <param name="qx">Measurement to position state minimal variance.</param>
/// <param name="qv">Measurement to velocity state minimal variance.</param>
/// <param name="r">Measurement covariance (sets minimal gain).</param>
/// <param name="pd">Initial variance.</param>
/// <param name="ix">Initial position.</param>
/**
*
* @param qx Measurement to position state minimal variance = accuracy of gps
* @param qv Measurement to velocity state minimal variance = accuracy of gps
* @param r Masurement covariance (sets minimal gain) = 0.accuracy
* @param pd Initial variance = accuracy of gps data 0.accuracy
* @param ix Initial position = position
*/
public void reset(double qx, double qv, double r, double pd, double ix){
m_q0 = qx; m_q1 = qv;
m_r = r;
m_p0 = m_p3 = pd;
m_p1 = m_p2 = 0;
m_x0 = ix;
m_x1 = 0;
}
public double getPosition(){
return m_x0;
}
public double getSpeed(){
return m_x1;
}
}
我正在使用两个一维过滤器，一个用于纬度，一个用于经度，然后在每次预测调用后从中构建一个新的位置对象。
我的初始化是qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy/10, pd = gpsAccuracy/10, ix = initial position.
我在从教程中获取代码后使用了这些值，这是他在评论中推荐的。
使用这个，我得到的速度是 a) 波动很大，b) 速度很慢，我得到的速度从 50 到几百 km/h 走路时，然后偶尔5-7，这更准确，但我需要速度保持一致，至少在一个合理的范围内。
试试这个简单的改变：
float speed = location.getSpeed() x 4;
GPS 接收器提供的 GPS 位置已经经过严格的 Kalman 滤波。如果位置仍在跳跃，则无法使用卡尔曼滤波器很好地解决该问题。 原因是低速移动不能很好地给出稳定的位置和速度（和方向） 只需删除所有低于 10km/h 的位置，将不再需要任何过滤。
我看到了几个问题：
- 您的
update()包含预测 和 更新，但您也有一个predict()，因此如果您实际调用predict()（你没有包括外循环）。 - 对于您的测量值是位置还是位置和速度存在一些混淆。您可以看到评论声称
H=[1,0]和H=[1,1]（他们可能表示H=[1,0;0,1]）因为矩阵数学是手写的，所以关于单一测量的假设被烘焙到所有矩阵中步骤，但代码仍会尝试 "measure" 速度。 - 对于从位置估计速度的 KF，您不想像那样注入合成速度（作为一阶差分）。让那个结果从钦哲基金会自然发生。对于
H=[1,0]，您可以看到K=PH'/S应该有 2 行，并且都适用于y0。这将同时更新x0和x1.
除了看看他们用 H 做了什么，我并没有真正检查矩阵数学。你真的应该用一个很好的矩阵库来开发这种算法（例如 numpy，用于 Python，或用于 C++ 的 Eigen）。当您进行微不足道的更改时（例如，如果您想尝试使用 2D 过滤器），这将为您节省大量代码更改，并避免简单的矩阵数学错误会让您抓狂。如果您必须针对完全手写的矩阵运算进行优化，请最后进行优化，以便比较您的结果并验证您的手写编码。
最后，关于您的具体应用，其他帖子完全正确：GPS 已经在过滤数据，输出之一是速度。