控制量由四部分組成，包括姿態(tài)控制內(nèi)回路和高度控制外回路的各兩個(gè)控制量。姿態(tài)控制內(nèi)回路：與俯仰角偏差△θ成正比的比例控制器作為主控制對(duì)俯仰角進(jìn)行修正，與俯仰角速率θ成正比的微分控制器主要用于改善姿態(tài)內(nèi)回路的阻尼特性。
高度控制外回路：與高度差△H成正比的比例控制器作為主控制對(duì)高度進(jìn)行修正，與垂向速度冉成正比的微分控制器則主要用于改善高度外回路的阻尼特性。
2．2 信號(hào)處理方法及效果分析
在基本控制律確定后，一個(gè)必須解決好的主要問題就是如何獲取高質(zhì)量的反饋控制信號(hào)。俯仰角偏差△θ與俯仰角速率θ，按常規(guī)做法可分別由垂直陀螺和角速率陀螺提供滿足精度要求的信號(hào)。這里主要問題在于高度信號(hào)H和垂向速度信號(hào)H的獲取和處理。無線電高度表信號(hào)由于海浪起伏等因素的影響本身含有大量的噪聲，并且如果直接采用高度表信號(hào)微分獲取垂向速度信號(hào)，會(huì)引起系統(tǒng)不穩(wěn)定，不能單獨(dú)使用；如果采用加速度計(jì)信號(hào)積分獲取垂向速度信號(hào)和高度信號(hào)，又存在誤差累積的問題，也不能單獨(dú)使用。然而如果將兩者組合使用，則可以產(chǎn)生很好的優(yōu)勢(shì)互補(bǔ)效果。所以，本文采用了一種Kalman互補(bǔ)濾波算法，既可濾除高度信號(hào)噪聲，又可獲得高質(zhì)量的垂向速度信號(hào)。其設(shè)計(jì)思想是通過對(duì)垂向加速度信號(hào)進(jìn)行積分得到垂向速度，再對(duì)垂向速度積分得到高度，將此高度與高度表的實(shí)際測(cè)量高度進(jìn)行比較，得到一個(gè)偏差量。把該偏差量作為當(dāng)前時(shí)刻的誤差估計(jì)值重新加入到垂直加速度和垂直速度上繼續(xù)進(jìn)行濾波運(yùn)算。高度信號(hào)與垂向加速度信號(hào)互補(bǔ)濾波的原理如圖3所示。

再對(duì)該濾波算法的效果進(jìn)行分析。設(shè)拖靶的實(shí)際高度為Hr，記a=Hr+Na，其中Na表示加速度的誤差，則有：

記H=Hr+Nh，Nh表示高度的誤差，則有：

從上面可以看出，解算后的速度V由三個(gè)部分組成：
真實(shí)高度的微分：sHr(s)

這樣得到的垂向速度和高度信號(hào)中，加速度誤差和高度誤差的信號(hào)能夠被有效地濾波，特別是能夠有效地去除海雜波的影響。