33 Eigen::MatrixXd K, Eigen::MatrixXd P)
35 double t = control_cycle;
36 double preview_size_ = round(preview_time/control_cycle) + 1;
52 c_ << 1, 0, -lipm_height/9.81;
54 Eigen::MatrixXd tempA = Eigen::MatrixXd::Zero(4,4);
55 Eigen::MatrixXd tempb = Eigen::MatrixXd::Zero(4,1);
56 Eigen::MatrixXd tempc = Eigen::MatrixXd::Zero(1,4);
58 tempA.coeffRef(0,0) = 1;
59 tempA.block<1,3>(0,1) = c_*A;
60 tempA.block<3,3>(1,1) = A;
62 tempb.coeffRef(0,0) = (c_*b).coeff(0,0);
63 tempb.block<3,1>(1,0) = b;
65 tempc.coeffRef(0,0) = 1;
71 Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4,4);
72 Q.coeffRef(0,0) = Q_e;
73 Q.coeffRef(1,1) = Q_e;
74 Q.coeffRef(2,2) = Q_e;
75 Q.coeffRef(3,3) = Q_x;
78 f.resize(1, preview_size_);
80 Eigen::MatrixXd mat_R = Eigen::MatrixXd::Zero(1,1);
81 mat_R.coeffRef(0,0) = R;
83 Eigen::MatrixXd tempCoeff1 = mat_R + ((tempb.transpose() * P) * tempb);
84 Eigen::MatrixXd tempCoeff1_inv = tempCoeff1.inverse();
85 Eigen::MatrixXd tempCoeff2 = tempb.transpose();
86 Eigen::MatrixXd tempCoeff3 = Eigen::MatrixXd::Identity(4,4);
87 Eigen::MatrixXd tempCoeff4 = P*tempc.transpose();
89 f.block<1,1>(0,0) = ((tempCoeff1_inv*tempCoeff2)* tempCoeff3) * tempCoeff4;
91 for(
int i = 1; i < preview_size_; i++)
93 tempCoeff3 = tempCoeff3*((tempA - tempb*K).transpose());
94 f.block<1,1>(0,i) = ((tempCoeff1_inv*tempCoeff2)* tempCoeff3) * tempCoeff4;
virtual ~PreviewControl()
Eigen::MatrixXd calcPreviewParam(double preview_time, double control_cycle, double lipm_height, Eigen::MatrixXd K, Eigen::MatrixXd P)