APM 姿态抗风(干扰 突变) 优化处理
问题描述：
将amp 移植到stm32 平台后，fast_loop执行频率 可以达到400hz，且可以使用卡尔曼滤波，飞行效果还可以，但gps 定点飞行时发现：有突变风的时候机体容易出现轻微摇晃，和大僵的对比，小动作明显多些，感觉没法接受。
同样的情况，在室内飞行时，飞机容易发生加速平移，加大积分参数后加速平移现象可以得到有效抑制，但新的情况：出现不稳定气流时，机体同样出现轻微摇晃，小动作频繁。经过数据分析，发现主要是大积分参数导致，且大积分参数还有其他各种负面影响。
经过各种实验和数据收集分析，得出以下结论:
1.导致这种现象的原因是外界气流的变化，作用于飞机产生姿态偏移。
2.飞机的抗风能力差，小动作频繁都是这个原因导致的。（这个抗风指的不是逆向抗风维持飞机位置，而是指突变风导致的姿态变化维持姿态稳定）
3.上述问题，最终归结于控制系统的干扰抑制问题，要解决这个问题，需要增强控制系统的干扰抑制能力。
4.通过调整PID 参数无法圆满的解决这个问题，通过Matlab 仿真，可以证明，纯PID 无法有效抑制干扰。
问题解决：
方案1
采用自抗扰PID设计，对于这种做法，关键在于观察器的误差精度，通过matlab 仿真，感觉效果不能满意，所以只作为备用方案。
方案2
采用H∞鲁棒控制，通过matlab仿真理想的H∞状态反馈控制效果堪称完美，对干扰的抑制极强。但实际情况是：首先是不稳定系统，其次是高灵敏系统（即输入到状态的变化很快，都是豪秒级别），再就是本系统属于不确定时滞系统。这样选取适合的H∞模型方程就很关键，本次打算首先采用这种方法尝试。
方案3
采用最优控制+干扰抑制，这方向尚未仔细研究，也没有做matlab仿真，作为备用方案。
方案4
自适应控制+观测器,具体待续。
方案2实施
首先，确定状态反馈阵，本次不详叙该过程。
采用的模型方程为: dX(t)=(A+△)X(t) + (B+△)u(t - d) + (Bw+△)w(t)
其次，修改apm 姿态控制算法，修改原则是尽量保证不破坏原有结构，方便以后其他算法无缝替换
以roll 定高为例先回顾一下姿态控制过程：
fast_loop()
attitude_control.rate_controller_run() (以下都在姿态控制对象中)
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x))根据当前角速度计算姿态控制输出
angle_ef_roll_pitch_rate_ef_yaw_smooth 函数，参数包含了目标姿态roll角度 (以下都在姿态控制对象中)
_angle_ef_target.x = roll_angle_ef 目标姿态roll角度
angle_ef_error.x = _angle_ef_target.x - _ahrs.roll_sensor计算角度差，这个我们用不到
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error) 坐标转换
update_rate_bf_targets()
_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x; 计算目标角速度， 这个我们用不到
H∞ 反馈控制对象
输入：当前roll角度当前roll角速度目标角度
输出： 控制输出
定义接口对象 AC_StateController 状态控制器，该对象虚函数 virtual float to_motor(float state1,float state2,float state3,float state4,float state5),该对象仅仅是
--实现状态反馈而已，所以非常简单。
根据以上控制流程，为了不破坏原有结构，以及兼容以后的新加控制算法，在AC_AttitudeControl中增加新的对象
AC_StateController* _rollController (初始值 NULL)
AC_StateController*_pitchController (初始值 NULL)
修改 rate_controller_run 函数为:
if(_rollController)
_motors.set_roll(_rollController->to_motor(_angle_bf_error.x,_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100));
else
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x));
if(_pitchController)
_motors.set_roll(_pitchController->to_motor( _angle_bf_errory,_ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100));
else
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
以上就完成了主要修改，完成细节部分后就可以直接测试了。
测试过程及现象，待续.
经过一段时间的调试，效果很差，具体就不描述了。
根据分析，感觉应该是模型方程不准确导致的，经过多次辨识，发现之前忽略了一些问题：系统可能是个时变系统
目前没有更好的办法，下一步打算采用 自适应控制：直接极点配置。