Java 卡尔曼滤波的GPS数据仍然波动很大

他是所有人

我正在编写一个Android应用程序，它使用GPS设备计算车辆行驶速度。这应该精确到1-2km/h，我通过观察两个GPS位置之间的距离除以这些位置分开的时间来计算，非常简单，然后对最后三个记录的坐标进行计算，最后得出结果

我在一个后台服务中获取GPS数据，该服务有一个到它自己的循环器的处理程序，因此每当我从LocationListener获得一个新位置时，我调用Kalmans update（）方法，并通过在predict（）之后调用sendEmptyLayedMessage来定期调用处理程序中的predict（）

我已经阅读并尝试在github中实现villoren在对该主题的回答中提供的过滤器，这也产生了波动的结果。 然后，我修改了本教程中的演示代码，我现在正在使用它。为了更好地理解过滤器，我手工做了所有的数学运算，我不确定我是否完全理解了他提供的源代码，但这就是我现在正在处理的问题：

我注释掉的那部分

/*// 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;
*/

在这里，我不同意所提供代码的数学公式，但考虑到（他说）他在火箭制导系统中实现了卡尔曼滤波器，我倾向于相信他的数学公式是正确的；）

公共类Kalman过滤器{
/*
X=状态
F=向前滚动X，通常为某个时间增量。
U=单位时间dt的值相加。
P=协方差–每件事物之间的差异。
Y=残差（测量和最后状态的增量）。
M=测量
S=协方差的残差。
R=最小创新协方差，防止过滤器锁定到解决方案中。
K=卡尔曼增益
Q=P的最小更新协方差，防止P变得太小。
H=实际转鼓到预测转鼓。
I=单位矩阵。
*/
//状态X[0]=位置，X[1]=速度。
私有双m_x0，m_x1；
//P=2x2矩阵，不确定度
私人双m_p0、m_p1、m_p2、m_p3；
//Q=最小协方差（2x2）。
私人双m_q0、m_q1、m_q2、m_q3；
//R=单个值。
私人双保险；
//H=[1，0]，我们只测量位置，所以没有状态更新。
私人最终双m_h1=1，m_h2=0；
//F=2x2矩阵：[1，dt]，[0，1]。
公共作废更新（双m，双dt）{
//预测到现在，然后更新。
//预测：
//X=F*X+H*U
//P=F*X*F^T+Q。
//更新：
//Y=M–H*X称为创新=测量–由H转换的状态。
//S=H*P*H^T+R S=剩余协方差=由H+R变换的协方差
//K=P*H^T*S^-1K=卡尔曼增益=方差/残差协方差。
//X=X+K*Y使用新测量值进行更新
//P=（I–K*H）*P此时更新协方差。
//X=F*X+H*U
双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]
双y0=m-m_x0；
双y1=（（m-oldX）/dt）-m_-x1；
//S=H*P*H^T+R
//因为H是[1,0]，s只是一个值
双s=m_p0+m_r；
/*//K=P*H^T*S^-1
双k=m_p0/s；
//双增益=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
双k0=m_p0/s；
双k1=m_p2/s；
//双增益=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；
}
公共空间预测（双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*/
}
/// 
///重置过滤器。
/// 
///测量到位置状态的最小方差。
///测量速度状态的最小方差。
///测量协方差（设置最小增益）。
///初始方差。
///初始位置。
/**
*
*@param qx测量到位置状态最小方差=gps精度
*@param qv测量速度状态最小方差=gps精度
*@param r Masurement协方差（设置最小增益）=0.5精度
*@param pd初始方差=gps数据精度0.5
*@param ix初始位置=位置
*/
公共无效重置（双qx、双qv、双r、双pd、双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；
}
公共职位{
返回m_x0；
}
公共双getSpeed（）{
返回m_x1；
}
}

我使用了两个1D过滤器，一个用于纬度，一个用于经度，然后在每次预测调用后用它们构造一个新的位置对象

我的初始化是qx=gpsAccuracy，qv=gpsAccuracy，r=gpsAccuracy/10，pd=gpsAccuracy/10，ix=初始位置

我在从教程中获得代码后使用这些值，这是他在评论中建议的

---

通过这种方法，我得到的速度a）波动很大，b）相差很远，我得到的速度是50-几百公里/小时，然后是偶尔的5-7公里/小时，这更准确，但我需要速度保持一致，至少在合理的范围内。

试试这个简单的变化：

float speed = location.getSpeed() x 4;

GPS定位，已交付

float speed = location.getSpeed() x 4;