机械臂筛选逆解数据——最大相关系数

//注意协方差也可以表示为相关性 ，但是不直观，所以用相关系数 把他锁在-1～1便于筛选
int group_num=flag_Q.size();
    vector<double> cov;
    for (int i = 0; i < group_num; i++)
    {
    //将逆解和初始条件求相关系数存在cov中
    cov.emplace_back(gama(qstart,flag_Q[i],6));
    }
  //找到机器人逆解中最大的相关系数
    auto max_cov =max_element(cov.begin(), cov.end());
      //找到最大相关系数的位置，是第几个
    int max_cov_pos=max_cov - cov.begin();

    //输出最小的一组解max_cov_pos存到q_out容器中 为机器人逆解中的最优解
    for (int i = 0; i < 6; i++)
    {
        q_out.emplace_back(flag_Q[max_cov_pos][i]);
    }
    
 //求最大相关系数，系数的范围为-1~1,越接近1说明关联程度越大，gama中参数为容器的引用传递
double  gama(vector<double> &A, vector<double> &B, long Length)
{

    double sumA(0.0), sumB(0.0), aveA(0.0), aveB(0.0);

    //求和
    sumA = std::accumulate(A.begin(), A.end(), 0.0);
    sumB = std::accumulate(B.begin(), B.end(), 0.0);

    //求平均值
    aveA = sumA / double(Length);
    aveB = sumB / double(Length);

    //计算相关系数
    double R1(0), R2(0), R3(0);
    for (long i = 0; i < Length; i++)
    {
        R1 += (A[i] - aveA) * (B[i] - aveB);
        R2 += pow((A[i] - aveA), 2);
        R3 += pow((B[i] - aveB), 2);
    }

    return (R1 / sqrt(R2*R3));
}

协方差与期望值有如下关系：
协方差：Cov(X，Y)=E(XY)-E(X)E(Y)
方差

参考：https://blog.csdn.net/MoreAction_/article/details/106195689
知：https://zhuanlan.zhihu.com/p/83410946