请问Apollo中方向盘输出使用的是什么算法进行滤波

Question

void LpfCoefficients(const double ts, const double cutoff_freq,
std::vector *denominators,
std::vector *numerators) {
denominators->clear();
numerators->clear();
denominators->reserve(3);
numerators->reserve(3);

double wa = 2.0 * M_PI * cutoff_freq; // Analog frequency in rad/s
double alpha = wa * ts / 2.0; // tan(Wd/2), Wd is discrete frequency
double alpha_sqr = alpha * alpha;
double tmp_term = std::sqrt(2.0) * alpha + alpha_sqr;
double gain = alpha_sqr / (1.0 + tmp_term);

denominators->push_back(1.0);
denominators->push_back(2.0 * (alpha_sqr - 1.0) / (1.0 + tmp_term));
denominators->push_back((1.0 - std::sqrt(2.0) * alpha + alpha_sqr) /
(1.0 + tmp_term));

numerators->push_back(gain);
numerators->push_back(2.0 * gain);
numerators->push_back(gain);
}

Answer 1

简单的二阶滤波，设置好截止频率然后离散就可以得到