制导控制2：协方差分析

1、问题描述
协方差分析，可以用来分析受随机输入驱动的时变线性系统。像伴随法一样，协方差分析是一种精确的分析方法，但仅限于线性系统。使用这种方法，系统状态向量的协方差矩阵会作为时间的函数，通过非线性矩阵微分方程的直接积分进行传播。利用该方法，可以得到任意状态或状态组合随时间变化的精确统计性能预测。协方差分析在惯性导航和最优估计相关问题中相当受欢迎。协方差分析技术也可以用来在导弹制导系统中获得精确的统计性能预测。
要应用协方差分析，需要将系统框图转换为状态空间表示，或者一个等价的以矩阵形式表示的一阶微分方程组。任何白噪声输入下的线性系统可以转换为以下一阶向量微分方程：
\(\dot{x(t)} = F(t)x(t)+u(t)\)
其中\(x(t)\)是系统状态向量，\(F(t)\)是系统动力学矩阵，\(u(t)\)是白噪声向量（白噪声的谱密度为\(Q(t)\)，\(Q(t) = E[u(t)u^T(t)]\)）。
这个系统协方差传播的矩阵微分方程为：
\(\dot{X(t)} = F(t)X(t)+[F(t)X(t)]^T+Q(t)\)
其中\(X(t)\)是与状态向量相关的协方差矩阵，\(X(t) = E[x(t)x^T(t)]\)。
如果干扰过程噪声的均值为零，协方差矩阵的对角线元素表示状态变量的方差。协方差矩阵的非对角线元素表示各个状态变量之间的相关程度。

2、例子
为了展示协方差分析在控制系统的实用性，采用一阶惯性环节的制导回路，如下图所示。在图中，输入\(u_s\)被整形滤波器等效物替代，也就是通过积分器的白噪声。

一阶惯性环节制导回路

首先，将框图转化为状态空间形式，转化时要将控制系统方程写为一系列一阶线性微分方程组。根据上框图可得：

\(\begin{matrix} \dot{y} = \dot{y}\\\ddot{y} = \ddot{y_T} - N'V_c\dot{D} - \frac{N'V_c}{T}[\frac{y}{V-c(t_f-t)-D}] \\\ddot{y_T} = u_s \\\dot{D} = \frac{1}{T}[\frac{y}{V-c(t_f-t)-D}] \end{matrix}\)

将方程组写为矩阵形式：

\( \begin{bmatrix} \dot{y} \\\ddot{y} \\\ddot{y_T} \\\dot{D} \end{bmatrix}=\begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{-N'}{T(t_f-t)} & 0 & 1 & \frac{-N'V_c}{T}\\ 0& 0 & 0 & 0 \\ \frac{1}{TV_c(t_f-t)}& 0 & 0 & \frac{-1}{T} \\ \end{bmatrix}\begin{bmatrix} y \\ \dot{y}\\\ddot{y_T} \\D \end{bmatrix}+\begin{bmatrix} 0 \\ 0\\ u_s\\0 \end{bmatrix} \)
系统的状态向量即为：
\( x=\begin{bmatrix} \dot{y} \\\ddot{y} \\\ddot{y_T} \\\dot{D} \end{bmatrix} \)
动力学矩阵为：
\(F=\begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{-N'}{T(t_f-t)} & 0 & 1 & \frac{-N'V_c}{T}\\ 0& 0 & 0 & 0 \\ \frac{1}{TV_c(t_f-t)}& 0 & 0 & \frac{-1}{T} \\ \end{bmatrix}\)
白噪声向量\(u(t)\)及其谱密度\(Q(t)\)为：
\(u=\begin{bmatrix} 0 \\ 0\\ u_s\\0 \end{bmatrix},Q= \begin{bmatrix} 0 & 0& 0& 0\\ 0&0 & 0 & 0 \\ 0& 0& \Phi_S & 0 \\ 0& 0 & 0 & 0 \\ \end{bmatrix},\Phi_S=n^2_T/t_f\)
对协方差分析非线性矩阵微分方程的积分可以得到所有状态的统计信息。对于这个制导回路示例，相对轨迹y的标准差可以通过取协方差矩阵\(X\)的第一个对角元素的平方根来求得\(\sigma _y(t)=\sqrt{X(1,1)}\)。

3、计算
采用四阶龙格库塔格式求解制导回路协方差传播的一阶微分方程组，初始条件如下：

clc;
T = 0.0;  %初始时刻
T_new = T;
H = 0.01;  %时间步长
XNP = 3.0;  %有效导航系数
Tau = 1.0;  %一阶惯性环节时间常数
XNT = 96.6;  %目标机动过载
VC = 4000;  %弹目相对运动速度
TF = 10.0;  %结束时刻
Tgo = TF - T + 0.00001;  %剩余飞行时间
phis = XNT^2/TF;  %白噪声谱密度

F = zeros(4,4);  %系统动力学矩阵
X = zeros(4,4);  %状态向量
Q = zeros(4,4);  %白噪声谱密度矩阵
F(1,2) = 1.0;
F(2,1) = -XNP/(Tau * Tgo);
F(2,3) = 1.0;
F(2,4) = XNP * VC/Tau;
F(4,1) = 1.0 / (Tau * VC * Tgo);
F(4,4) = -1.0 / Tau;
Q(3,3) = phis;

对于一阶微分方程 \(\dot{x}=f(x,t)\)，四阶龙格库塔格式计算公式：
\(x_k+1 = x_k+h/6(K_0+2K_1+2K_2+K_3)\)
k表示当前值，k+1表示下一步值，其中：
\(\begin{matrix} K_0=f(x_k,t_k) \\K_1=f(x_k+0.5K_0,t_k+0.5h) \\K_2=f(x_k+0.5K_1,t_k+0.5h) \\K_3=f(x_k+0.5K_2,t_k+h) \end{matrix}\)
计算环节如下：

k = 1;
while ~(T >= (TF-.0001))  %截止条件
    X_old0 = turnvar1( X );  %将上一步的状态向量存在X_old0

    Tgo = TF - T_new + 0.00001;  %时间迭代
    XD = turnvar2( F , X_old0 , Q , XNP , Tau, Tgo , VC);  %根据X_old0计算斜率K0
    K0 = XD;

    T_new = T + 0.5*H;
    X_old1 = turnvar3(X_old0, K0,H);  %计算预测步1的状态向量

    Tgo = TF - T_new + 0.00001;
    XD = turnvar2( F , X_old1 , Q , XNP , Tau, Tgo , VC);  %根据X_old1计算斜率K1
    K1 = XD;

    T_new = T + 0.5*H;
    X_old2 = turnvar3(X_old0, K1,H);  %计算预测步2的状态向量

    Tgo = TF - T_new + 0.00001;
    XD = turnvar2( F , X_old2 , Q , XNP , Tau, Tgo , VC);  %根据X_old2计算斜率K2
    K2 = XD;

    T_new = T + H;
    X_old3 = turnvar4(X_old0, K2,H);  %计算预测步3的状态向量

    Tgo = TF - T_new + 0.00001;
    XD = turnvar2( F , X_old3 , Q , XNP , Tau, Tgo , VC);  %根据X_old3计算斜率K3
    K3 = XD;

    T = T_new;
    Xnew =  turnvar5( X_old0 , K0, K1, K2, K3,H);  %带入四阶龙格库塔格式
    X = Xnew;

    Y = turnvar6( X , XNP,Tau,Tgo,VC );  %计算nL的协方差

    
    save(k,1) = T;
    save(k,2) = sqrt(X(1,1));
    save(k,3) = sqrt(Y);
    k = k+1;
end
plot(save(:,1),save(:,2),'LineWidth',3),grid

function OLD = turnvar1(var)
    OLD = var;
end

function XD = turnvar2(F,X,Q,XNP,Tau,Tgo,VC)
    F(2,1) = - XNP/(Tau * Tgo);
    F(4,1) = 1.0/(Tau * VC * Tgo);
    FX = F * X;
    FXT = FX';
    FXFXT = FX + FXT;
    XD = FXFXT + Q;
end

function OLD = turnvar3(X_old, K0,H)
    OLD = zeros(4,4);
    for i = 1:4
        for j = 1:4
            OLD(i,j) = X_old(i,j) + 0.5 * H *K0(i,j);
        end
    end
end

function OLD = turnvar4(X_old, K0,H)
    OLD = zeros(4,4);
    for i = 1:4
        for j = 1:4
            OLD(i,j) = X_old(i,j) + H *K0(i,j);
        end
    end
end

function OLD = turnvar5(X_old, K0, K1, K2, K3,H)
    OLD = zeros(4,4);
    for i = 1:4
        for j = 1:4
            OLD(i,j) = X_old(i,j) + H * ( K0(i,j) + 2.0 * ( K1(i,j)+K2(i,j) ) + K3(i,j) )/6.0;
        end
    end
end

function Y = turnvar6(X,XNP,Tau,Tgo,VC)    %计算nL的协方差矩阵Y
    A=zeros(1,4);
    A(1) = XNP / ( Tau * Tgo );
    A(2) = 0.0;
    A(3) = 0.0;
    A(4) = -XNP*VC/Tau;

    AT = A';
    Y = A * X * AT;
end

4.计算结果
下图表示导弹与目标之间相对距离的最终标准差，协方差分析程序显示未命中距离的标准偏差为13.3英尺。