123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140 |
- /*
- * @Description: 测量结果滤波器
- * @Author: yct
- * @Date: 2021-03-15 14:41:46
- * @LastEditTime: 2021-03-16 16:30:08
- * @LastEditors: yct
- */
- #ifndef MEASURE_FILTER_HH
- #define MEASURE_FILTER_HH
- #include "../tool/singleton.h"
- #include "../tool/common_data.h"
- #include "../error_code/error_code.h"
- #include <map>
- #include <deque>
- #include <chrono>
- #include <vector>
- #include <algorithm>
- #include "glog/logging.h"
- #define FILTER_SIZE 10
- #define MAX_QUEUE_SIZE 15
- #define MAX_TIME_INTERVAL_MILLI 7000
- class Measure_filter : public Singleton<Measure_filter>
- {
- // 子类必须把父类设定为友元函数,这样父类才能使用子类的私有构造函数。
- friend class Singleton<Measure_filter>;
- public:
- // 必须关闭拷贝构造和赋值构造,只能通过 get_instance 函数来进行操作唯一的实例。
- Measure_filter(const Measure_filter &) = delete;
- Measure_filter &operator=(const Measure_filter &) = delete;
- ~Measure_filter() = default;
- // 更新测量数据回调
- void update_data(int terminal_id, Common_data::Car_wheel_information_stamped data)
- {
- if(!data.wheel_data.correctness)
- return;
- // LOG(INFO) << data.wheel_data.to_string();
- // 未创建队列
- if (m_measure_results_map.find(terminal_id) == m_measure_results_map.end())
- {
- std::deque<Common_data::Car_wheel_information_stamped> t_deque;
- t_deque.push_back(data);
- m_measure_results_map.insert(std::pair<int, std::deque<Common_data::Car_wheel_information_stamped>>(terminal_id, t_deque));
- // LOG(INFO) <<"queue inserted";
- }
- else//已创建
- {
- m_measure_results_map[terminal_id].push_back(data);
- // LOG(INFO) <<"data pushed "<<m_measure_results_map[terminal_id].size();
- // 剔除超时数据
- while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now()-m_measure_results_map[terminal_id].front().measure_time).count() > MAX_TIME_INTERVAL_MILLI)
- {
- m_measure_results_map[terminal_id].pop_front();
- }
- // 维持队列长度
- while(m_measure_results_map[terminal_id].size() > MAX_QUEUE_SIZE)
- {
- m_measure_results_map[terminal_id].pop_front();
- }
- }
- }
- bool has_enough_data(int terminal_id)
- {
- if(m_measure_results_map.find(terminal_id) != m_measure_results_map.end() && m_measure_results_map[terminal_id].size() >= FILTER_SIZE)
- return true;
- else
- return false;
- }
- // 获取滤波后最新测量数据
- Error_manager get_filtered_wheel_information(int terminal_id, Common_data::Car_wheel_information& result)
- {
- // 检查数据量
- if(m_measure_results_map.find(terminal_id) == m_measure_results_map.end() || m_measure_results_map.find(terminal_id)->second.size() < FILTER_SIZE)
- {
- return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR,
- (std::string("缺少足够用于滤波的结果")+
- std::to_string(terminal_id)+
- std::string(m_measure_results_map.find(terminal_id) == m_measure_results_map.end()?
- "end":
- std::to_string(m_measure_results_map.find(terminal_id)->second.size()))).c_str());
- }
- // 填充待滤波结果到数组
- std::deque<Common_data::Car_wheel_information_stamped>* tp_deque = &m_measure_results_map.find(terminal_id)->second;
- std::vector<Common_data::Car_wheel_information_stamped> t_result_vec;
- Common_data::Car_wheel_information_stamped t_avg_result;
- for (std::deque<Common_data::Car_wheel_information_stamped>::reverse_iterator t_iter = tp_deque->rbegin(); t_iter != tp_deque->rend(); t_iter++)
- {
- t_result_vec.push_back(*t_iter);
- t_avg_result += *t_iter;
- if (t_result_vec.size() >= FILTER_SIZE)
- break;
- }
- if(t_result_vec.size()<FILTER_SIZE)
- return Error_manager(WJ_FILTER_LACK_OF_RESULT, MINOR_ERROR, "结果缺失");
- // 各结果计算平均,并打分
- t_avg_result /= t_result_vec.size();
- std::vector<float> t_score_vec, t_sorted_score_vec;
- for (size_t i = 0; i < t_result_vec.size(); i++)
- {
- t_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
- t_sorted_score_vec.push_back((t_result_vec[i] - t_avg_result).calc_score());
- }
- // 排序,丢掉最高两个(即与均值偏差最大两个,剩下平均获得最终值)
- std::sort(t_sorted_score_vec.begin(), t_sorted_score_vec.end());
- Common_data::Car_wheel_information_stamped t_final_result;
- int t_result_count = 0;
- for (size_t i = 0; i < t_score_vec.size(); i++)
- {
- if(t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-1] || t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-2])
- {
- continue;
- }else{
- t_final_result += t_result_vec[i];
- t_result_count++;
- }
- }
- if(t_result_count<=0)
- return Error_manager(WJ_FILTER_FLUCTUATING, MINOR_ERROR, "结果波动");
- t_final_result /= t_result_count;
- result = t_final_result.wheel_data;
- // LOG(INFO) << "\navg: \n\t"<<t_avg_result.wheel_data.to_string()<<"final: \t"<<t_final_result.wheel_data.to_string();
- return SUCCESS;
- }
- private:
- // 父类的构造函数必须保护,子类的构造函数必须私有。
- Measure_filter() = default;
- // 各终端测量数据队列
- std::map<int, std::deque<Common_data::Car_wheel_information_stamped> > m_measure_results_map;
- };
- #endif // !MEASURE_FILTER_HH
|