1) cleaned up servo_filtering. servo_shift moved into servo_filter and only do if err signal is valid
2) Fixed dtrat>1 case for LGS. Was not working error due to uplink error signal not reset when there
should be no output.