px4ctrl里estimateThrustModel代码详解

了解这段代码需要有最小二乘法的基础，可以参考我上一篇博客

https://blog.csdn.net/howard789/article/details/140742869

bool 
LinearControl::estimateThrustModel(
    const Eigen::Vector3d &est_a,
    const Parameter_t &param)
{
  ros::Time t_now = ros::Time::now();
  while (timed_thrust_.size() >= 1)
  {
    // Choose data before 35~45ms ago
    std::pair<ros::Time, double> t_t = timed_thrust_.front();
    double time_passed = (t_now - t_t.first).toSec();
    if (time_passed > 0.045) // 45ms
    {
      // printf("continue, time_passed=%f\n", time_passed);
      timed_thrust_.pop();
      continue;
    }
    if (time_passed < 0.035) // 35ms
    {
      // printf("skip, time_passed=%f\n", time_passed);
      return false;
    }

    /***********************************************************/
    /* Recursive least squares algorithm with vanishing memory */
    /***********************************************************/
    double thr = t_t.second;
    timed_thrust_.pop();
    
    /***********************************/
    /* Model: est_a(2) = thr1acc_ * thr */
    /***********************************/
    double gamma = 1 / (rho2_ + thr * P_ * thr);  // 公式一
    double K = gamma * P_ * thr;   // 公式二
    thr2acc_ = thr2acc_ + K * (est_a(2) - thr * thr2acc_); // 公式三
    P_ = (1 - K * thr) * P_ / rho2_; // 公式四
    //printf("%6.3f,%6.3f,%6.3f,%6.3f\n", thr2acc_, gamma, K, P_);
    //fflush(stdout);

    // debug_msg_.thr2acc = thr2acc_;
    return true;
  }
  return false;
}

以下是四条公式的推导

double gamma = 1 / (rho2_ + thr * P_ * thr);  //公式一
double K = gamma * P_ * thr;   //公式二
thr2acc_ = thr2acc_ + K * (est_a(2) - thr * thr2acc_); //公式三
P_ = (1 - K * thr) * P_ / rho2_; //公式四

这个thr2acc是你自己试飞后得到的估计值，默认值0.3，P的初始值是1e6，可以理解为过去用于计算初始thr2acc的样本所累计的值.