连续系统的控制律表达式为：

U=K1*[Kp(Yg-Y)+Ki*∫(Yg-Y)dt+Kd*(Yg′-Y′)-K2*x]-K*x

离散化增量形式：

ΔU(k)=
U(k)-U(k-1)=K1*[Kp*(e(k)-e(k-1))+KiTe(k)+Kd*(e(k)-2e(k-1)+e(k-2))/T+K2*(x(k-1)-x(k))]+K(x(k-1)-x(k))
=K1*[(Kp+Ki*T+Kd/T)*e(k)-(Kp+2Kd/T)*e(k-1)+Kd/T*e(k-2)+K2*(x(k-1)-x(k))] +K(x(k-1)-x(k))
 
其中k为采样序号，e(k)=Yg(k)-Y(k)
我的程序如下：
double * CWDSIMUDlg::ControlStep(double *v)
{
   
 int Cmd=2;
 
 double temp1[4],temp2[4];
 double temp3[4],temp4[4];
 double temp5[4],temp6[4];
 double temp7[4],delta_u[4];
 double delta_y[4],delta_x[12];
 int i, j;
 int ii, jj, kk;
 
 for(i=0; i<4; i++)
 {
         temp1[i]=0;
         temp2[i]=0;
     temp3[i]=0;
         temp4[i]=0;
         temp5[i]=0;
         temp6[i]=0;
         temp7[i]=0;
         delta_u[i]=0;
 }
 i=0;
 
   
 switch(Cmd){
 case 0:                              
         break;
 case 1:                              
     break;
 
 case 2:                          
    if((m_t >= 0) && (m_t < 25.13))
    {
            m_Plane.xg=20*sin(0.25*m_t);
            m_Plane.yg=(-20)*cos(0.25*m_t)+20;
            m_Plane.zg=1*m_t;
            m_Plane.psig=0.25*m_t;
 
    }
    else if((m_t>=25.13) && (m_t < 50.26))
    {
            m_Plane.xg=20*sin(6.2825)+20*sin(0.25*(m_t-25.13));
            m_Plane.yg=20*cos(0.25*(m_t-25.13))-20;
            m_Plane.zg=1*m_t;
            m_Plane.psig=6.2825-0.25*(m_t-25.13);
 
    }
    else if((m_t>=50.26) && (m_t < 75.39))
    {
            m_Plane.xg=2*20*sin(6.2825)+20*sin(0.25*(m_t-50.26));
            m_Plane.yg=(-20)*cos(0.25*(m_t-50.26))+20;
            m_Plane.zg=1*m_t;
            m_Plane.psig=0.25*(m_t-50.26);
 
    }
    else
    {
            m_Plane.xg=3*20*sin(6.2825)+20*sin(0.25*(m_t-75.39));
            m_Plane.yg=20*cos(0.25*(m_t-75.39))-20;
            m_Plane.zg=1*m_t;
            m_Plane.psig=6.2825-0.25*(m_t-75.39);
 
    }
 
    Enow[0] = m_Plane.xg-v[9];
    Enow[1] = m_Plane.yg-v[10];
    Enow[2] = m_Plane.zg-v[11];
    Enow[3] = m_Plane.psig-v[8];
/*         delta_u=K1*[Know*Enow+Kpast*Epast+Kprev*Eprev+(Yddnow-Yddpast)+K2*(Xpast-Xnow)]
                +K*(Xpast-Xnow);     */
  for(i=0; i<4; i++)
  {
          for(j=0; j<4; j++)
          {
                 temp1[i] += Know[i][j] * Enow[j];
                 temp2[i] += Kpast[i][j] * Epast[j];
                 temp3[i] += Kprev[i][j] * Eprev[j];
        }
  }
    i=0;
    j=0;
 
    for(ii=0; ii<12; ii++)
    {
          delta_x[ii]=Xpast[ii]-v[ii];
    }
    ii=0;
 
    for(jj=0; jj<4; jj++)
    {
            for(kk=0; kk<12; kk++)
            {
                   temp4[jj] += K2[jj][kk] * delta_x[kk];
                   temp5[jj] += K[jj][kk] * delta_x[kk];
            }
    }
    jj=0;
    kk=0;
 
    for(i=0; i<4; i++)
    {
            temp6[i]=temp1[i]+temp2[i]+temp3[i]+temp4[i];         

    }
    i=0;
 
    for(i=0; i<4; i++)
    {
            for(j=0; j<4; j++)
            {
                   temp7[i] += K1[i][j] * temp6[j];            
            }
    }
       i=0;
    j=0;
 
    delta_u[0]=temp7[0]+temp5[0];
    delta_u[1]=temp7[1]+temp5[1];
    delta_u[2]=temp7[2]+temp5[2];
    delta_u[3]=temp7[3]+temp5[3];
 
   
    for(jj=0; jj<4; jj++)
    {
            Eprev[jj]=Epast[jj];
            Epast[jj]=Enow[jj];
    }
    jj=0;
 
    for(kk=0; kk<12; kk++)
    {
            Xpast[kk]=v[kk];
    }
    kk=0;
 
   
    m_x[0] += delta_u[0];
    m_x[1] += delta_u[1];
    m_x[2] += delta_u[2];
    m_x[3] += delta_u[3];
 
    break;
 
    default:
            break;
 }
 
 
 for(int nCtrlStep=0;nCtrlStep<4;nCtrlStep++)
       m_ArrayControlStep[nCtrlStep]=m_x[nCtrlStep];
 return m_ArrayControlStep;
}
程序进行到1点几秒结果就不对了，求出的控制量U和其他变量都是非常大的值，表现在控制中就是发散了，请大家帮忙看看，程序哪有问题吗？