PX4 悬停油门预估算法

相关论文：

Adaptive Adjustment of Noise Covariance in Kalman Filter for Dynamic State Estimation

相关资料：

悬停油门的参数估计方法_px4悬停油门-CSDN博客

大概思路(ps:遗忘了很多细节)：

        1、油门更新为噪声驱动，其次由加速度计算悬停油门；

        2、PX4使用的是AEKF，自适应卡尔曼滤波。

问题发现：

        自己虚拟的油门、加速度数据，测试过程中，加速度计相关的R矩阵会先增大再收敛。

 Matlab中关键更新部分实现：

    % updateLpf(residual, signed_innov_test_ratio)
    alpha = dt / (lpf_time_constant + dt);
    residual_lpf = (single(1) - alpha) * residual_lpf + alpha * residual;
    signed_innov_test_ratio_lpf = (single(1) - alpha) * signed_innov_test_ratio_lpf + alpha * signed_innov_test_ratio;
    
    % updateMeasurementNoise(residual, H)  
    beta = dt / (noise_learning_time_constant + dt) ;
    res_no_bias = residual - residual_lpf;
    P = state_var;
    acc_var = (single(1) - beta) * acc_var  + ...           % 更新R矩阵
                beta * (res_no_bias * res_no_bias + H * P * H);