randomctrl.m

组合导航的程序

%The homework of random control written by xiaogang
%The optimal estimation of the ship'attitude.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   A = [ 0	    1	     0	 0	        0	   0	    0	      0	   0	
         0    -2.09    0   -3.38     1.63   57.23  0.84   57.23  0.84
         1  	0	     0	  1	      0	   0	     0	   0	    0
         0	  0.33	  0	-0.77	   -0.17	   0.32	3.06	 0.32	  3.06
         0	   0	     0	 0	      -1.85	    0	     0	   0	    0
         0	   0	     0	 0				0		 0	     0	   0	    0
         0	   0		  0	 0	         0	    0	     0	   0	    0
         0	   0	     0	 0	         0	    0	     0	 -0.28	 0
         0	   0	     0	 0	         0	    0	     0	    0 	-0.28
       
       ];%9*9 matrix
   
   temp_C = [ 0	0	0	0	0	0	0	1	0
              0	0	0	0	0	0	0	0	1  ];
   C =temp_C'; %9*2
   H = [1	0	0	0	0	0	0	0	0
        0	1	0  0	0	0	0	0	0
        0	0  1  0	0	0	0	0	0
       ];      %3*9        
       
   Q = [1 0
        0 1];
   R = [1 0	0
        0 1 0
        0 0 1]; 
     
       T = 10;    %the interval of sample
     phi = eye(9);%initialize the matrix
     gama1 = 10*eye(9);
      
   tmpi = 1;
   for i = 1:100
      tmpi = tmpi*i;   
      phi = phi+(A*T)^i/tmpi;
       gama1 = gama1+(-A*T)^i*T/(tmpi*(i+1));
   %   gama1 = gama1 + (A*T)^i*T/(tmpi*(i+1));

   end
    gama = phi*gama1*C;    
   %gama = gama1*C;    

   
   posai = eye(9,9);
   An =  eye(9);
   Bn =  eye(9);
    P0 = zeros(9);
    X0 = zeros(9,1);
     Pw = P0;
     Xk = X0;
     Xk1 = X0; 
    Xgu = X0;
    for k =1:100   %%the kalman filter
       W = randn(2,1);%W and v are random in every loop!
       V = randn(3,1);
       Pyc = phi*Pw*phi'+gama*Q*gama';
        Kk = Pyc*H'*inv(H*Pyc*H'+R);
       pw = [eye(9)-Kk*H]*Pyc;
       Xyc = phi*Xgu;
       Xk = phi*Xk+gama*W;
       Uk = -inv(posai'*An*posai+Bn)*posai'*An*phi*Xgu;
       %Uk = -eye(6,9)*phi*Xgu;
       Xk1 = phi*Xk1+0.1*gama*W+posai*Uk;
       Zk = H*Xk+V;
       Zw = Zk-H*Xyc;
       Xgu = Xyc+Kk*Zw;
       y1(k) = Xgu(8); 
       y2(k) = Xk(8); 
       y3(k) = Xk1(8);
   end
   x = linspace(1,100,100);
   plot(x,y1,'r',x,y3)