贝叶斯估计射速-有兴趣可以去了解，不是重点内容

 "bayesEstimateVelocity.h"

#ifndef BAYESESTIMATEVELOCITY
#define BAYESESTIMATEVELOCITY

#include<iostream>
#include<vector>
class BayseEstimate {
public:
    BayseEstimate(float velocityMean = 17.0f, float velocityStd = 1.0f,
                  float goodProb = 0.9f, float badProb = 0.1f,
                  int strainCapacity = 1, int filterCapacity = 2,
                  std::string meanAndStdSavePath = "");

    /**
     * @brief  strTrim
     * @author author
     * @date   2018-
     */
    static std::string strTrim(std::string s);

    /**
     * @brief  getMean
     * @return velocityMean
     * @author author
     * @date   2018-
     */
    float getMean();

    /**
     * @brief  gaussian
     * @return gaussian
     * @author author
     * @date   2018-
     */
    float gaussian(float x, float u, float std);

    /**
     * @brief  bayesCompare
     * @param  x
     * @return gaussian()
     * @author 2018-
     */
    float bayesCompare(float x);

    /**
     * @brief  statisticAbnormalProb
     * @param  historyList
     * @return count / float(historyList.size())
     * @author author
     * @date   2018-
     */
    float statisticAbnormalProb(std::vector<std::vector<float>> historyList);

    /**
     * @brief  calMeanAndStd
     * @param  historyList
     * @param  onlyCalNormalVelocity
     * @return meanAndStd
     * @author author
     * @date   2018-
     */
    float *calMeanAndStd(std::vector<std::vector<float>>historyList, bool onlyCalNormalVelocity=false);

    /**
     * @brief  getbayesEstimateVelocity
     * @param  latestVelocity
     * @return estimateMean
     * @author author
     * @date   2018-
     */
    float getbayesEstimateVelocity(float latestVelocity);

private:
    float goodProb = 0.9;
    float badProb = 0.1;
    float velocityMean = 17;
    float velocityStd = 1.0;
    int updateParams[2] = {2,2};
    std::vector<std::vector<float>> bufferList;
    int runTimes = 0;
    std::string meanAndStdSavePath = "velocityMeanAndStd.txt";
    bool meanOrStdHaveUpdate = true;
};

#endif

bayesEstimateVelocity.cpp

#include "bayesEstimateVelocity.h"
#include<math.h>
#include<fstream>
#include<sstream>
using namespace std;

BayseEstimate::BayseEstimate(float velocityMean, float velocityStd, float goodProb,
                             float badProb, int strainCapacity, int filterCapacity,
                             std::string meanAndStdSavePath)
{
    this->goodProb = goodProb;
    this->badProb = badProb;
    this->velocityMean = velocityMean;
    this->velocityStd = velocityStd;
    this->updateParams[0] = strainCapacity;
    this->updateParams[1] = filterCapacity;
    this->meanAndStdSavePath = meanAndStdSavePath;
    stringstream stream;
    ifstream in(this->meanAndStdSavePath);
    string line;
    if(in)
    {
        while(getline(in, line))
        {
            if(strTrim(line)!= "")
            {
                int index = line.find(' ');
                this->velocityMean = atof(line.substr(0, index).c_str());
                this->velocityStd = atof(line.substr(index + 1).c_str());
                break;
            }
        }
    }
}

std::string BayseEstimate::strTrim(std::string s)
{
    int beginIndex = -1;
    int endIndex = -1;
    for(size_t i = 0; i < s.length(); i++)
    {
        if (s[i] != ' ' && s[i] != '\n')
        {
            beginIndex = i;
            break;
        }
    }
    for(size_t i = s.length() - 1; i >= 0; i--)
    {
        if(s[i] != ' ' && s[i] != '\n')
        {
            endIndex = i;
            break;
        }
    }
    if(beginIndex != -1)
        return s.substr(beginIndex, endIndex - beginIndex + 1);

    return "";
}

float BayseEstimate::getMean()
{
    return velocityMean;
}

float BayseEstimate::gaussian(float x, float u, float std)
{
    if(std <= 0) std = 0.1f;
    return expf(-((x - u) * (x-u)) / (2.0 * (std * std)));
}

float BayseEstimate::bayesCompare(float x)
{
    return gaussian(x, velocityMean, velocityStd) * goodProb - (1 - gaussian(x, velocityMean, velocityStd * 1.2)) * badProb;
}

float BayseEstimate::statisticAbnormalProb(vector<vector<float>> historyList){
    int count = 0;
    for(int i = 0; i < historyList.size(); i++)
    {
        if(historyList[i][1] < 0) count++;
    }
    return count / float(historyList.size());
}

float *BayseEstimate::calMeanAndStd(vector<vector<float>> historyList, bool onlyCalNormalVelocity)
{
    static float meanAndStd[2];
    float mean = 0;
    float std = 0;
    int count = 0;
    for(int i = 0; i < historyList.size(); i++)
    {
        if(onlyCalNormalVelocity && historyList[i][1] < 0)
            continue;
        mean += historyList[i][0];
        count++;
    }
    mean /= count;
    for(int i = 0; i < historyList.size(); i++)
    {
        if(onlyCalNormalVelocity && historyList[i][1] < 0)
            continue;
        std += ((historyList[i][0] - mean) * (historyList[i][0] - mean));
    }
    if (count == 1) std /= count;
    else std /= (count - 1);
    meanAndStd[0] = mean;
    meanAndStd[1] = sqrtf(std);
    return meanAndStd;
}

float BayseEstimate::getbayesEstimateVelocity(float latestVelocity)
{
    float estimateMean = velocityMean;
    float estimateStd = velocityStd;
    if(bufferList.size() == 0 || latestVelocity != bufferList[bufferList.size() - 1][0])
    {
        float bayesCompareValue = bayesCompare(latestVelocity);
        if(bayesCompareValue != 0)
        {
            vector<float>velocityParam = {latestVelocity, bayesCompareValue};
            bufferList.push_back(velocityParam);
            if(bufferList.size() % updateParams[0] == 0)
            {
                if(statisticAbnormalProb(bufferList) >= 0.01)
                {
                    float *estimateMeanAndValue = calMeanAndStd(bufferList, false);
                    estimateMean = estimateMeanAndValue[0];
                    estimateStd = estimateMeanAndValue[1];
                    velocityMean = estimateMean;
                    velocityStd = estimateStd;
                    bufferList.clear();
                    meanOrStdHaveUpdate = true;
                }
                else
                {
                    if(bufferList.size() % updateParams[1] == 0)
                    {
                        float *estimateMeanAndValue = calMeanAndStd(bufferList, true);
                        estimateMean = estimateMeanAndValue[0];
                        estimateStd = estimateMeanAndValue[1];
                        velocityMean = estimateMean;
                        velocityStd = estimateStd;
                        meanOrStdHaveUpdate = true;
                    }
                }

            }
        }
        if(runTimes % 5 == 0 && meanOrStdHaveUpdate == true && this->meanAndStdSavePath != "")
        {
            ofstream OutFile(meanAndStdSavePath);
            OutFile << to_string(velocityMean) + " " + to_string(velocityStd);
            OutFile.close();
            meanOrStdHaveUpdate = false;
        }
        runTimes++;
    }
    return estimateMean;
}

 

 

贝叶斯估计类涉及文件（bayesEstimateVelocity.cpp、bayesEstimateVelocity.h）。

代码文件有个bayesEstimateVelocity文件，是炜涛师兄19赛季写来预测弹丸发射初速度的，配合重力补偿用。不过用起来需要了解代码，能对存储文件进行修改。我大概看了看代码，里面用的应该是最小误判概率准则，是最小损失准则的特例，都是模式识别课程的内容，准则基于贝叶斯原理，有兴趣可以去了解，不是重点内容。弹丸初速度其实可以直接用电控给的数据，有误差可以接受（注意：让电控发射速的话，是上一次发弹的射速，不是本次发弹的射速，因为还没发出去，不可能测）。