卡尔曼滤波的GPS数据仍然波动很大 [英] Kalman-filtered GPS data is still fluctuating a lot
问题描述
他是所有人！
我正在编写一个使用GPS设备计算车辆行驶速度的Android应用程序。这应该是精确到大约1-2公里/小时，我通过查看两个GPS位置之间的距离并将它们除以这些位置分开的时间来完成它，非常简单，然后为此最后三个记录的坐标，然后晚上出来。
我在后台服务中获取GPS数据，它拥有自己的循环处理程序，所以每当我获得新位置从LocationListener中，我调用Kalmans update（）方法，并通过在predict（）之后调用sendEmptyDelayedMessage来定期在处理程序中调用predict（）。
I已阅读平滑的GPS数据，并且实际上也尝试在vithoren提供的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 =滚动X向前，通常是一些时间增量。
U =每单位时间增加值dt。
P =协方差 - 每个事物相互之间的变化情况。
Y =剩余（测量和最后状态的增量）。
M =计量单位
S =协方差的残差。
R =最小的创新协方差，使滤波器不会锁定到解决方案。
K =卡尔曼增益
Q = P的最小更新协方差，使得P不会太小。
H =实际卷到预测。
I =单位矩阵。
* /
//状态X [0] =位置，X [1] =速度。
private double m_x0，m_x1;
// P =一个2x2矩阵，不确定性
私有双m_p0，m_p1，m_p2，m_p3;
// Q =最小协方差（2x2）。
private double m_q0，m_q1，m_q2，m_q3;
// R =单个值。
private double m_r;
// H = [1,0]，我们只测量位置，所以没有状态更新。
private final double m_h1 = 1，m_h2 = 0;
// F = 2x2矩阵：[1，dt]，[0，1]。
public void update（double m，double dt）{
//预测到现在为止，然后更新。
//预测：
// X = F * X + H * U
// P = F * X * F ^ T + Q.
//更新：
// Y = M - H * X称为创新=测量 - 由H转换的状态
// S = H * P * H ^ T + RS =残差协方差=由H + R转换的covarine
// K = P * H ^ T * S ^ -1 K =卡尔曼增益=方差/残差协方差。
// X = X + K * Y更新并获得新的测量值
// P =（I - K * H）* P更新此时的协方差。
// 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
//为了获得速度的变化，我们假装也测量速度，
//使用H作为[1 ，1]
double y0 = m - m_x0;
double y1 =（（m - oldX）/ dt） - m_x1;
// S = H * P * H ^ T + R
//因为H是[1,0]，s只有一个值
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;
}
公共无效预测（double dt）{
// X = F * X + H * U将状态（X）滚动到新时间。
m_x0 = m_x0 +（dt * m_x1）;
// P = F * P * 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; * /
}
///< summary>
///重置过滤器。
///< / summary>
///< param name =qx>测量位置状态最小方差。< / param>
///< param name =qv>测量到速度状态最小方差< /参数>
///< param name =r>测量协方差（设定最小增益）。< / param>
///< param name =pd>初始方差。< / param>
///< param name =ix>初始位置。< / param>
$ b $ ** b
$ b * @param qx测量位置状态最小方差= GPS精度
* @param qv速度状态测量最小方差=精度gps
* @param r测量协方差（设置最小增益）= 0.accuracy
* @param pd初始方差= GPS数据的准确度0.accuracy
* @param ix初始位置=
$ /
public void reset 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;
}
}
我使用两个1D滤波器，一个用于纬度，一个用于经度，然后在每次预测调用之后构建一个新的位置对象。
我的初始化是qx = gpsAccuracy，qv = gpsAccuracy，r = gpsAccuracy / 10，pd = gpsAccuracy / 10，ix =初始位置。
我使用教程之后的值我得到了代码，这是他推荐的评论。
使用这个，我得到的速度是a）波动很大，b）速度是有效的，我的速度从50到几百公里/小时，然后也偶尔5-7，这是更准确，但我需要的速度是一致的，至少在一个合理的范围内。
- 您的
update（）包含predict 和更新，但是您还有一个predict（），所以您会双重集成ve如果你真的调用了predict（）（你没有包含外部循环），那么它就是locity。 - 测量是位置或位置和速度。你可以看到声明
H = [1,0]和H = [1,1]的评论可能意味着H = [1,0; 0,1]）由于矩阵数学是手写的，所以关于单次测量的假设被烘焙到所有矩阵步骤，但代码仍然试图测量速度。
- 对于从位置估计速度的KF，您不希望像这样注入合成速度（作为一阶区别）。让这个结果自然发生在KF上。对于
H = [1,0]，你可以看到K = PH'/ S应该有2行，并且都适用于y0。这将更新x0和x1。
除了看看他们用 H 做了什么以外，我没有真正检查矩阵运算。你应该真的用一个好的矩阵库来开发这种算法（例如，numpy，Python，或Eigen for C ++）。当您进行微小的更改（例如，如果您想试验2D滤镜）并避免简单的矩阵数学错误会使您发疯时，这将为您节省大量代码更改。如果你不得不优化到完全手写的矩阵运算，那么最后这样做，这样你就可以比较你的结果并验证你的手写代码。
He everyone!
I'm writing an Android app that uses the devices GPS to calculate a vehicles driving speed. This is supposed to be accurate to about 1-2 km/h, and I'm doing it by looking at the distance between two GPS locations and dividing it by the time these locations are apart, pretty straightforward, and then doing this for the last three recorded coordinates and evening it out.
I get the GPS data in a background service, that has a handler to it's own looper, so whenever I get a new location from the LocationListener, I call the Kalmans update() method and call the predict() in a handler in regular intervals by calling sendEmptyDelayedMessage to itself after the predict()
I have read Smooth GPS data and have actually also tried implementing the filter in the github that was provided by villoren in an answer to that topic, which also yielded fluctuating results. I have then adapted the demo code from this tutorial http://www.codeproject.com/Articles/326657/KalmanDemo, which I am working with right now. I did all the maths by hand to better understand the filter, and I'm not sure if I understood his provided source code completely, but this is what I am working with right now:
The part where I commented out
/*// 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;
*/
is where I didn't agree with the maths of the code provided, but given (he states) he has implemented Kalman filters in rocket guidance systems I am inclined to believe his maths is right ;)
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;
}
}
I'm using two 1D Filters, one for latitude and one for longitude and then construct a new location object out of that after each predict call.
My initialization is qx = gpsAccuracy, qv = gpsAccuracy, r = gpsAccuracy/10 , pd = gpsAccuracy/10, ix = initial position.
I use the values after the tutorial I got the code from, this is what he recommended in the comments.
Using this, I get speeds that are a) fluctuating a lot, and b) speeds that are WAY off, I get speeds from 50 - a few hundred km/h while walking, and then also the occasional 5-7, which is more accurate, but I need the speeds to be consistent and at least in a sensible range.
I see a few issues:
- Your
update()contains predict and update, but you also have apredict(), so you would be double-integrating velocity if you actually callpredict()(you didn't include the outer loop). - There's some confusion as to whether your measurement is position or position and velocity. You can see comments claiming
H=[1,0]andH=[1,1](by which they probably meantH=[1,0;0,1]) Since the matrix math is hand-written, the assumption about the single measurement is baked into all of the matrix steps, but the code still tries to "measure" velocity as well. - For a KF which estimates velocity from positions, you do not want to inject synthetic velocity like that (as a first-order difference). Let that result occur naturally from the KF. For
H=[1,0], you can see howK=PH'/Sshould have 2 rows, and both apply toy0. That will update bothx0andx1.
I didn't really check the matrix math other than to see what they had done with H. You should really develop this kind of algorithm with a nice matrix library (e.g. numpy, for Python, or Eigen for C++). That will save you a lot of code changes when you make trivial changes (e.g. if you want to experiment with a 2D filter) and avoid simple matrix math errors that will drive you mad. If you have to optimize to fully hand-written matrix operations, do it last so you can compare your results and verify your hand coding.
And finally, the other posts are entirely correct about your specific application: The GPS is already filtering the data, and one of the outputs is velocity.
这篇关于卡尔曼滤波的GPS数据仍然波动很大的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！
