使用卡尔曼滤波器从GPS位置过滤速度

Question

我尝试使用卡尔曼滤波器对GPS数据计算出的速度进行滤波。 这是我的卡尔曼滤波器类：

    #include "SimpleKalmanFilter.h"
    #include <math.h>

    SimpleKalmanFilter::SimpleKalmanFilter(double mea_e, double est_e, double q)
    {
        _err_measure = mea_e;
        _err_estimate = est_e;
        _q = q;
    }

    double SimpleKalmanFilter::updateEstimate(double mea)
    {
        _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
        _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate);
        _err_estimate = (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate - _current_estimate)*_q;
        _last_estimate = _current_estimate;

        return _current_estimate;
    }

以下是该类的用法：

    void cal_minmax_speed_by_gps(std::vector<na_data>* na, float& min_speed_by_gps, float& max_speed_by_gps) {
        writeV << "velocity,distance,time" << std::endl;
        std::vector<na_data>& na_ptr = *na;
        SimpleKalmanFilter skm = SimpleKalmanFilter(1, 1, 0.01);
        double total_dis = 0, total_time = 0;
        for (int i = 0; i < na->size() - 1; i++) {
            double distance = calc_dist(na_ptr[i].latitude, na_ptr[i].longtitude, na_ptr[i + 1].latitude, na_ptr[i + 1].longtitude);
            total_dis += distance;      
            double time = (na_ptr[i + 1].time - na_ptr[i].time).total_seconds();       
            total_time += time;
            na_ptr[i].speed_by_gps = (distance / time)*3.6 ;

            na_ptr[i].speed_by_gps = skm.updateEstimate(na_ptr[i].speed_by_gps);

            if (na_ptr[i].speed_by_gps < 1) na_ptr[i].speed_by_gps = speed_temp;            
        }

        auto min_max = std::minmax_element(na->begin(), na->end(), [](const na_data& n1, const na_data& n2) {return n1.speed_by_gps < n2.speed_by_gps; });
        na_data& min_speed = *min_max.first;
        na_data& max_speed = *min_max.second;
        min_speed_by_gps = min_speed.speed_by_gps;
        max_speed_by_gps = max_speed.speed_by_gps;  

    }

但是似乎卡尔曼滤波器不起作用。过滤后的数据看起来与过滤前的数据相同。

原始速度

滤波速度：

我初始化卡尔曼滤波器是否错误？我该如何解决？