фильтровать скорость от местоположения GPS, используя фильтр Калмана - PullRequest
Новая
       34

фильтровать скорость от местоположения GPS, используя фильтр Калмана

0 голосов
/ 07 июня 2019

Я пытался использовать фильтр Калмана для фильтрации скорости, рассчитанной по данным 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;  

    }

Но похоже, что фильтр Калмана не работает. Данные после фильтра выглядят одинаково перед фильтрацией.

Скорость сырья:

raw velocity

Скорость фильтрации: filtered velocity

Я неправильно инициализировал фильтр Калмана? Как я могу это исправить?

...