1. .52. ....72.1. , -
....................................................................................................................7
2.1.1. ..........................7 2.1.1.1
..........................................................7 2.1.1.2
......................................................11 2.1.1.3
............................................................13
2.1.2. -
...................................................................................14
2.1.2.1. .....14 2.1.2.2. .....14 2.1.2.3.
............................................................15
2.1.2.4. .......................162.2. ....17 2.2.1
.................................................17 2.2.1.1
..............................17 2.2.1.2
.................................20 2.2.1.3 . -
......................................................................21
2.2.2
-................................................................................................24
2.2.2.1 ......................24 2.2.2.2 , ,
.............................................................................25
2.2.2.3 ...........................................302.3. ....34
2.3.1 -
.......................................................................35
2.3.1.1
.............................................................................35
2.3.1.2 ................................................36 2.3.1.3.
...............................40 2.3.2 -
...........................................................................45
2.3.2.1 ...............................................45 2.3.2.2
...............................................47 2.3.2.3 -
.....................................................................482.4.
..54 2.4.1. ......54 2.4.2. ......712.5. ...76
3. - ...773.1.
.........................................................................773.2.
.............................................................................793.3.
............................................................833.4.
.........854. ..88
4.1.......................................................................................................88
4.2
.................................................................................88
4.2.1.
...................................................................88
4.2.2
..................................................................................89
4.2.3
............................................................89
4.2.4. ................................................91 4.2.5
...................................................................92
4.2.6 .......................................................94
4.3. ........................................................96 4.4
...................................................................................................995.
...1006. 101
1. . , - . , - , (, , , , .), . , , . , , , . ( ), (, , , ), (),
, .. . . . , . . A. : , ; ; ; () ;
2. .2.1 , 2.1.1. -. , JIA, . . , , : ( ); ; ; ; ; , .
2.1.1.1 . F(.2.1.). Po, HP. dH dP, dQ dV, F: dP = dQ /
F.(2.1)
. 2.1. dQ dV g . , dQ=gFdH,: dP= -gdH. (2.2) , . P, R = 29,27 /
T: g=P / RT. (2.3) g , : dP / P = - dH / RT. (2.4) , . . .
. 2.2. , , 11000 , , 33000 (.2.2.). : T= T- tH, (2.5) - ;t-
,/;H- ,. T (2.4), PP, - 0H:
P,:
(2.6) . . 11000 (T11). (2.2) P11P, 11000 H:
(2.7)P11- 11000. . , , . - , : 0 11000 :
; (2.8) 11000 20000 :
. (2.9) , : P, PT(P11T11) -t. P(P11),T(T11) t, . (). . .
2.1.1.2
. 2.3. 2 : 1 2
() . ( ) , = ( t)/2, . ; = 3108/ , . ( ). :- ;- ;- ;- ;- .
. 2.4.
(2.10) (2.11) (2.12) (2.13)S ;a ;f0 ; 01000 . - . , (), . .
. 2.5. ( ), .. .: - , , f0 , , , , .
2.1.1.3 .
2.1.2. - .2.1.2.1.
- .A (.2.6), , , v V :
(2.14)
. 2.6. 2.1.2.2.
(2.15), dt,
(2.16) :
(2.17) . 1% , .2.1.2.3. . , . - :
(2.18) , JIA. - , . , .2.1.2.4. . , , , , , ( ) A , .
(2.19)
(2.20) .2.7.
. 2.7. ,
2.2 , , . , , , , . , , . , , . , () . .2.2.1 2.2.1.1 . . :
, (2.21)
x(t) - n- ; - - ; u(t) - r- ; F(t), G(t), C(t) - nn, np, nr ,
.
F(t), G(t), C(t) - , . , .
. , , u(t), , x(t1). . , .
. (1) :
(2.22)
- nn, :
. (2.23)
, :
, I - , . (2.26) , F. , x , x(t) , . . F, G,
F
(2.24)
- :
. (2.25)
(2.28) , . , (2.23). (2.21)
(2.26) . . , , , . , , , (2.24). , . , , . :
(2.27)
, - , . (2.24). x(t), , u(t) F(t), G(t), C(t):
2.2.1.2 . :
(2.28)
k=0,1,2,... - , ; (k) - n- ; w(k) - - ; u(k) - r- ; (k + 1,k) -
nn; (k+1,k), (k + 1,k) - n nr, .
(k + 1,k), (k+1,k), (k + 1,k) , . :
x(0), (1,0), (1,0), (1,0), w(0) u(0), 0, 1:
x(1), (2,1), (2,1), (2,1), w(1), u(1), 2:
.2.2.1.3 . . . , . , . . (.2.8).
.2.8 .
, , (2.28). , , k+1 , k. , , .
(2.29) k+1, , k.
, (2.32) . , .
(2.25) :
-. ,
:
(2.30)
(2.31)
(k + 1,k), (k+1,k), (k + 1,k) :
(2.32)
(2.33)
(2.34)
2.2.2 .2.2.2.1 . (): y(1),,y(j), 1,,j, x(k) k, . , x(k)
y(1),,y(j). (2.35) . (a) x(k) k 1,,k. , ; (c) j x(k) k 1,,j, jk.
.
.2.9
2.2.2.2 , , . . :
(2.36) u(k) , , , , , . . - (). :
, (2.37)
, m- ;
, m- ;H(k+1) :
. (2.38) 1 0. , =1. , =0. . :1. w(k) k Q(k), p x p.2. z(k+1) k
R(k+1), m x m.3. x(0) P(0), n x n.4. w(k), z(j) x(0) k j. . :
- n- k+1; - k+1;
;
- , ; ; P(k+1/k+1) ;
;
- ; - .
, P(k+1/k+1) :1) , (2.39)2) , (2.40)3) , (2.41)4) , (2.42)
nn; np; - pp; - mm; - mn; - nm; - nn.
. (2.39) - (2.42). , (k+1=0) . [x(0)] = 0. P(0). , : ;
P(0/0)=P(0).
1 (k=0), (1,0), (1,0), (1), Q(0), R(1), P(0/0), , y(1) :
(2.42):
, (2.40):
, (2.39):
, (2.41):
,
. P(1/1) .
2 (k=1), (2,1), (2,1), (2), Q(1), R(2), P(1/1), y(2), .
. : P(k+1/k+1) , P(k/k) y(k+1).
P(0/0) . , , . . , x(k). , P(k+1/k+1). , , . (k+1=0) , , . 1
(k=0) . , , . , . , , . . u(k), k. :
, (2.43)
, (2.44)
(2.45)
, (2.46)
, (2.47)
: ; P(0/0)=P(0).
2.2.2.3
. , , x(t0) , y(t), t0 t t1 t1. t0, . , . , , .
, nmn
(2.48) n, n ., , .
. , , :
(2.49)
. t0, u(t), t0 t t1, x(t0) x(t1)=0. x(t0) t0, . , u(t). .
, nnr
(2.50)
n, n . ,
(2.51)
. (t)=Fx(t), y(t)=Hx(t) , x(t)=Fx(t)+Gw(t), y(t)=Hx(t)+z(t). . (
(0)) ( ) P(k/j). , . P(k/j).
. , R(t) G(t)Q(t)GT(f) , P(k/j) , . , . , , . , . , . , (0) . ,
. , .
2.3. . . . . . . . , . .
2.3.1 - .2.3.1.1 - , V , , . :
. :
-
. . :
(2.52)
2.3.1.2 , , o, . , (2.52)
; (2.53)
(2.54)
(2.28)
, :
(2.55)
(2.56)
. (2.57)
:
:
:
(2.58) G ,
(2.59) : = + z, (2.60) y - ; z - - , ;
, (2.61)
- ; z - - :
, , , :
- 3, n =3. , .
. :
- 2, n =3. , . (2.43), (2.45), (2.46), (2.47) (2.47).
,
,
,
.
:
.2.10 2.3.1.3. MatLab, , , . - . , , .
, . 2 4 .
- /2.
.2.11
- /2 (g=10).
.2.121- :
=0.01, . . 2- :
=0.01, . . 3- :
=0.1, . . 4- :
=0.1, . .
: (0) (0) - .
(t) V(t) . (0/ 0) .
. ,V . , :
:
.
:
- k+1;
- k+1; - .
.
x(k+1)=
, (2.46), (2.47), (2.48), (2.49) (2.50).
.
: 1 (k=0 , k+1 =1 ) :
2 (k=1 , k+1 =2):
2.3.2. - . , (). z, . , . . . .2.3.2.1 . . :
. .
z :
(2.62) , z ( ), y .
(2.63) y = x +z , ; a = -1/. ; b = 1/. . (2.63) (2.24). . y. a
F, b - (2.24)., (2.27) :
.
, , :
:
(2.64)
2.3.2.2 , :
.2.13
. . :
1 . :
2 .:
1:
1 :
2 :
2.3.2.3 .
.
, . 2 4 .
- /2.
.2.14
- /2 (g=10).
.2.151- :
=0.01 , . , .2- :
=0.01 , . , .
: (0) (0) - .
(t) V(t) .
.- , :
z (2.61) :
. (2.23) :
;
: , .
:
, , .- :
. .
:
- k+1;
- k+1; - .
c .
, (2.46), (2.47), (2.48), (2.49) (2.50).
:
: 1 (k=0 , k+1 =1 ) :
2 (k=1 , k+1 =2):
2.4. 2.4.1. .a. .
.2.16
.2.17
- 1-
.2.18
.2.19 , .
.2.20 .
.2.21 .- 2-
.2.22
.2.23 , .
.2.24 .
.2.25 .
- 3-
.2.26
.2.27 , .
.2.28 .
.2.29 .
- 4-
.2.30
.2.31 , .
.2.32 .
.2.33 .
. .
.2.34
.2.35 - 1-
.2.36
.2.37 , .
.2.38 .
.2.39 .
- 2-
.2.40
.2.41 , .
.2.42 .
.2.43 .
- 3-
.2.44 , .
.2.45 .
.2.46 .
- 4-
.2.47 , .
.2.48 .
.2.49 .
2.4.2. .a. . - 1-
.2.50 , .
.2.51 .
.2.52 .
- 2-
.2.53 , .
.2.54 .
.2.55 .
. .
.2.56 , .
.2.57 .
.2.58 .
2.5. 1. a .2. .3. - ( , ) .
3. .. . . - (). : - . - . - . - . - .3.1. -. - (- ) , , .
3.1. - . - - , , , ,
- () 0,457
0,457
0,237
- - (J):
(3.1)
- - , ,
- , .
(3.2)
- i- , , ,
- () i- , .
(3.3)
- i- , , . 2.1 J 1,52.
3.2 . , , , 3.2.1 . . , . . :
, (3.4)
j- , .-.;
, 8 ;
, j- , . 1,4.
3.2 - / (, ) , % , - (. )
1. 54024
2. , 54024
3. 108027
4. 108027
5. 25200135
6. 20150213
7.
108035
8 - 104017
9. 55019
10058091
: 200 - - 5 : 40 - 1-, 2- 8-
3.1 : : 35 - 5 : 7 1- 2-
3.3. . 3.3.
- (-)i , , ,, i,
1234567894477351357922221231110001000100010001000100010001000100020002000200020001000200030001000100080008000140001400035000260001500070009000
91136000
. = 136000[.] , :.. = 136000 * 0,35 = 47600 [.] , (3.5) : =
13600 * 1,20 = 163200 [.] , (3.6) , , 10% : = 136000 * 0,1 =
13600[.] , (3.7) , : = . +.. + + =
=136000+47600+163200+13600=360400[.] (3.8) ( ) :
, (3.9)
, , - ( ), . ( =200400%)
=360400+136000200/100 = 632400 [.] 2.4. .
(.) (%)
1. 2. 3. 4. 136000476001632001360037,713,245,33,8
:360400100
3.4. . : (), (), (). - (). , , .3.4.1 . () . , (, , ..) (.
- , .;
- , ;
-
:
3.4.2
()
- , .,
- , . ( =0,35 )
3.4.3 = 2,443.5.:
- . - . - 580 [. .] , 360400 .
4. - 4.1 - . - (), , . - . 4.2 4.2.1. . , : . 4.1. ( )
12.1.005-88 (2001) -
, , , % , /
122 - 2440-600,2
123- 2540-600,2
303 . 4.2. 4.2
16-28 oC13-30 oC
%40-80 %30 -90 %
, /0,10,1
2. , , . 4.2.2 . 1 Samsung ML-1210. 50 . 12.1.003-83 . , - 50 .
.. .
4.2.3 . . ( + ). , -, () 1.2% , 1.5% - ( ). 23-05-95. . , . 200
, . ( ). 200 . . . . ( ), : 80-90% ; 50-60%; 15-20%; 15-30%. , , .
, , , . . . , . . , , . , -.
4.2.4. . . , , , ( ), . , . (, ). , - , , . () 10 /2 ( ) . . -
2.2.2/2.4.1340-03. , : 4.3
50 . 10 / 8 /
50 .0,3 /0,1 /
50923-96.4.2.5 .- 50923-96 - 20 3 , 4,5 2 .- - 25,48 3 , 5,8 2 .
50923-96 - 2.2.2/2.4.1340-03 - 680 - 800 . - 800, 1000, 1200 1400 ,
- 800 1000 , 725 . : - 725 . -1400 . - 1000 . 2.2.2/2.4.1340-03. -
- 400 . - 400 - 550 . 350 - 550 . -300 +-20 , - 380 .
2.2.2/2.4.1340-03 250 -50 -70 . 230 +-30 - 350 - 500 . : 450 . 320
, - 400 . 2.2.2/2.4.1340-03 +-30 . 30 . 290 , 60 . 250 450.
2.2.2/2.4.1340-03.4.2.6 :- 50923-96 . . . 500 . . 60 .
2.2.2/2.4.1340-03 35 /2. 2.2.2/2.4.1340-03 3:1. 2.2.2/2.4.1340-03
34. 2.2.2/2.4.1340-03 70 /2. . 500 . 50 . 56 . () 300 x 388 x 183 .
300 /2. 800:1. 50, 2.2.2/2.4.1340-03. 120 /2. 1600x900. 0.264 .
50923-96 2.2.2/2.4.1340-03.
4.3. . . , , , , 23-05-95. 23-05-95 200 . , , , :1. ; 2. ;3. ;4.
. . . ., 17677-82 . , - 12 ., .
, . 24 2. 6, 4 . 4.3. .
. 4.1.
3 . 12 -18 18 1350 . . :
, , ; E , S , 2; z , 1,1; , , 1,5; N ; , , , , 0,71.
, A B , h () . S, 2 24Z 1.1 1.5N, . 12 0,77
:
> 200 300 23-05-95.
4.4 , , . -- . - -18, 18 1350 .
5. 1. .. : . .- .:- , 1994. 72 .: .1. .. . . .: , 1983.-224.,.1.
.. . - . - .:- , 2003.1. .., .. : . - .:- , 1998. 268 .: .1. .. : -
.: , 1981.-344.,.1. .., .., .. . . 3-, . . ., , 1978, . 432.1. .. .
, ,1970.
6. 6.1. .6.1.1. .%
.%===============================================================================clc;clear;n=45;
%time% T=0.1; % (c )sigmaz=1; % (c )a=10; % aa=0.1; %
k=0:1:(n/T-1); t=k*T; %
Hg=0.5*(((k+1)*T).^2)*a;V=(k+1)*T*a;figure;plot(t,Hg,'r-','LineWidth',2);grid
on;hold on;plot(t,V,'b-','LineWidth',2);xlabel(' t()');ylabel('
H_() V_(/)');legend('H_','V_',2);title(' '); %================
==================z= sigmaz*randn(1,n/T); % for i=
0:1:(n/T-1)Y(1,i+1)=Hg(1,i+1)+ z(1,i+1); % end%==========
================================sigma_delta_a=0.05 ; % deltaA =
sigma_delta_a*randn(1,n/T); for i =0:1:(n/T-1)a(1,i+1)=10 +
deltaA(1,i+1)+ aa;end%=======================
=================Phi=[1 T -0.5*(T^2) ; 0 1 (-T); 0 0 1]; %
Ksi=[0.5*(T^2) ; T ; 0]; % Ge = -Ksi; % H=[1 0 0]; % % x00= [0;0;0]
; P00=[0 0 0 ; 0 0 0 ; 0 0 0.0025];P = cell(n/T,n/T);x=
cell(n/T,n/T); % , (k/k)Q=0.05^2;%
1====================================================================P10=Phi*P00*transpose(Phi)+Ge*Q*transpose(Ge);A1=P10*transpose(H)*((H*P10*transpose(H)+sigmaz^2)^(-1));x10=Phi*x00
+ Ksi*a(1,1);x{1,1}=x10 + A1*(Y(1,1)-
H*x10);P{1,1}=(eye(3,3)-A1*H)*P10; %C 2-
=============================================================for i
=2:1:n/TP{i,i-1}=Phi*P{i-1,i-1}*transpose(Phi)+Ge*Q*transpose(Ge);Ai=P{i,i-1}*transpose(H)*((H*P{i,i-1}*transpose(H)+sigmaz^2)^(-1));x{i,i-1}=Phi*x{i-1,i-1}
+ Ksi*a(1,i);x{i,i}=x{i,i-1} + Ai*(Y(1,i)-
H*x{i,i-1});P{i,i}=(eye(3,3)-Ai*H)*P{i,i-1};end % result_id =
fopen('E:\hoc tap\diplom\hoan thanh\diplom\result2.doc','w');%
%'E:\hoc tap\diplom\hoan thanh\diplom\result2.doc'=> %
fprintf(result_id,' variant : T = %1.4f , sigmaz = %3d m
\n',[T,sigmaz]);fprintf(result_id,'t(s)| H_d(m)| H_op(m)| sigma_H|
delta_H \n');fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %6d| %6.3f| %5.3f|
%6.3f\n',[i*T ; Hg(1,i);x{i,i}(1,1) ;
sqrt(P{i,i}(1,1));Hg(1,i)-x{i,i}(1,1)]);A(1,i*T)=sqrt(P{i,i}(1,1));
end fprintf(result_id,' \n');fprintf(result_id,'t(s)| V(m/s)|
V_op(m/s)| sigma_V| delta_V \n');fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %6d| %6.3f| %5.3f| %6.3f
\n',[i*T ; V(1,i);x{i,i}(2,1) ;
sqrt(P{i,i}(2,2));V(1,i)-x{i,i}(2,1)]);B(1,i*T)=sqrt(P{i,i}(2,2));endfprintf(result_id,'
\n');fprintf(result_id,'t(s)| ^a(m/s)| ^a_op(m/s)| sigma_^a|
delta_^a \n');fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %0.2f| %6.3f| %5.3f| %6.3f
\n',[i*T ; aa ;abs(x{i,i}(3,1)) ;
sqrt(P{i,i}(3,3));aa-x{i,i}(3,1)]);C(1,i*T)=sqrt(P{i,i}(3,3));endfclose(result_id);for
i=1:1:n/THop(1,i)= x{i,i}(1,1);Vop(1,i)= x{i,i}(2,1);end;% ,%
figure;i=(3/T):(4/T); grid on;hold
on;plot(k*T,Y,'green-','LineWidth',2);plot(k*T,Hg,'blue-','LineWidth',2);plot(k*T,Hop,'r-','LineWidth',2);xlabel('
t()');ylabel(' H_()
_(/)');legend('H_','_','H_o_z_e_n_k_a',2);title(' ');
%======================================================figure;clf;tt=1:n;plot(tt,A,'r-*');grid
on;hold on;plot(tt,B,'b-');plot(tt,C,'k-+')xlabel('
t()');ylabel('CKO_\Delta_H() ,
CKO_\Delta_V(/),CKO_\Delta_\Delta_a(/2)
');legend('CKO_\Delta_H','CKO_\Delta_V','CKO_\Delta_\Delta_a');title('
. . \Deltaa');% figure;i=1:n/T;plot(k*T,z,'b-');grid on;hold
on;plot(i*T,Hg-Hop,'r-','LineWidth',2);xlabel(' t()');ylabel('
()');legend(' ',' ');title(' '); %
====================================figure;i=1:n/T;plot(i*T,Hg-Hop,'r-','LineWidth',1);grid
on;xlabel(' t()');ylabel(' ()');legend(' ');title(' '); %
==================================figure;i=1:n/T;plot(i*T,V-Vop,'r-','LineWidth',1);grid
on;xlabel(' t()');ylabel(' (/)');legend(' ',4);title(' ');
6.1.2. .%
.%======================================================================clc;clear;n=45;%
T=0.1; % g=10; % g aa=0.1 ; % k=1:1:(n/T); t=k*T;% Hg =
5*g*t/(0.2*pi)-5*g*(sin(0.2*pi*t))/(0.2*pi)^2;V =
5*g/(0.2*pi)-5*g*cos(0.2*pi*t)/(0.2*pi);%======================================================figure;plot(t,Hg,'r-','LineWidth',2);grid
on;hold on;plot(t,V,'k-');xlabel(' t()');ylabel(' '
);legend('','');%=========== =====sigmaz=1; Z= sigmaz*randn(1,n/T);
for i= 0:1:(n/T-1) Y(1,i+1)=Hg(1,i+1)+ Z(1,i+1);end%==========
=============sigma_delta_a=0.05 ;deltaA =
sigma_delta_a*randn(1,n/T); for i =0:1:(n/T-1)
a(1,i+1)=5*g*sin(0.2*pi*(i+1)*T) + deltaA(1,i+1)+aa;end%===========
============Phi=[1 T -0.5*(T^2) ; 0 1 (-T); 0 0 1]; Ksi=[0.5*(T^2)
; T ; 0]; Ge = -Ksi; H=[1 0 0]; % x00= [0;0;0] ; P00=[0 0 0 ; 0 0 0
; 0 0 0.025];P = cell(n/T,n/T);x= cell(n/T,n/T);%
1==============================P10=Phi*P00*transpose(Phi)+Ge*(sigma_delta_a^2)*transpose(Ge);A1=P10*transpose(H)*((H*P10*transpose(H)+sigmaz^2)^(-1));x10=Phi*x00
+ Ksi*a(1,1);x{1,1}=x10 + A1*(Y(1,1)-
H*x10);P{1,1}=(eye(3,3)-A1*H)*P10; %C 2-
=====================================for i
=2:1:n/TP{i,i-1}=Phi*P{i-1,i-1}*transpose(Phi)+Ge*(sigma_delta_a^2)*transpose(Ge);Ai=P{i,i-1}*transpose(H)*((H*P{i,i-1}*transpose(H)+sigmaz^2)^(-1));x{i,i-1}=Phi*x{i-1,i-1}
+ Ksi*a(1,i);x{i,i}=x{i,i-1} + Ai*(Y(1,i)-
H*x{i,i-1});P{i,i}=(eye(3,3)-Ai*H)*P{i,i-1};end result_id =
fopen('E:\hoc tap\diplom\hoan thanh\diplom\result3.doc','w');%
%'result3.doc'=> % fprintf(result_id,' variant : T = %1.3f ,
sigmaz = %3d m \n',[T,sigmaz]); fprintf(result_id,'t(s)| H_d(m)|
H_op(m)| sigma_H| delta_H \n'); fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %6.3f| %6.3f| %5.3f|
%6.3f\n',[i*T ; Hg(1,i);x{i,i}(1,1) ;
sqrt(P{i,i}(1,1));Hg(1,i)-x{i,i}(1,1)]);
A(1,i*T)=sqrt(P{i,i}(1,1));end fprintf(result_id,' \n');
fprintf(result_id,'t(s)| V(m/s)| V_op(m/s)| sigma_V| delta_V \n');
fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %6.3f| %6.3f| %5.3f| %6.3f
\n',[i*T ; V(1,i);x{i,i}(2,1) ;
sqrt(P{i,i}(2,2));V(1,i)-x{i,i}(2,1)]);
B(1,i*T)=sqrt(P{i,i}(2,2));end fprintf(result_id,' \n');
fprintf(result_id,'t(s)| ^a(m/s)| ^a_op(m/s)| sigma_^a| delta_^a
\n'); fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %0.2f| %6.3f| %5.3f| %6.3f
\n',[i*T ; aa ;abs(x{i,i}(3,1)) ;
sqrt(P{i,i}(3,3));aa-abs(x{i,i}(3,1))]);
C(1,i*T)=sqrt(P{i,i}(3,3));end fclose(result_id);for i=1:1:n/T
Hop(1,i)= x{i,i}(1,1); Vop(1,i)= x{i,i}(2,1);end;% %
figure;i=(3/T):(4/T); grid on;hold
on;plot(k*T,Y,'green-','LineWidth',2);plot(k*T,Hg,'blue-','LineWidth',2);plot(k*T,Hop,'r-','LineWidth',2);xlabel('
t()');ylabel(' H_()
_(/)');legend('H_','_','H_o_z_e_n_k_a',2);title(' ');
%======================================================
figure;clf;t=1:n;plot(t,A,'r-','LineWidth',2);grid on;hold
on;plot(t,B,'k--');plot(t,C,'b-+')xlabel('
t()');ylabel('CKO_\Delta_H() ,
CKO_\Delta_V(/),CKO_\Delta_\Delta_a(/2)
');legend('CKO_\Delta_H','CKO_\Delta_V','CKO_\Delta_\Delta_a');title('
. . \Deltaa');% figure;i=1:n/T;plot(i*T,Z,'b-');grid on;hold
on;plot(i*T,Hg-Hop,'r-','LineWidth',2);xlabel(' t()');ylabel('
()');legend(' ',' ');title(' '); %
====================================figure;i=1:n/T;plot(i*T,Hg-Hop,'r-','LineWidth',1);grid
on;xlabel(' t()');ylabel(' ()');legend(' ');title(' '); %
==================================figure;i=1:n/T;plot(i*T,V-Vop,'r-','LineWidth',1);grid
on;xlabel(' t()');ylabel(' (/)');legend(' ',4);title(' ');
6.2. .6.2.1. .%
.%==========================================================================clc;clear;n=45;%
T=0.01; % g=10; % aa=0.1; % C=0.05; % sigmaz=1; k=0:1:(n/T);t=k*T;%
==============================================Hg=0.5*g*(t.^2);V=g*t;for
i=1:1:n/T HH(1,i)=Hg(1,i);
VV(1,i)=V(1,i);end;E=1-exp(-T/C);B=exp(-T/C);%===============YY(k)======================================================Z=
sigmaz*randn(1,n/T+1);for k1=1:1:n/T+1YY(1,k1)= 0.5*(k1*T)^2*g +
0.5/C*g*(-2*C^2*k1*T+2*C^3-2*C^3*exp(-1/C*k1*T));end%==========
==============================sigma_delta_a=0.05 ;% deltaA =
sigma_delta_a*randn(1,n/T+1); for i =0:1:(n/T) a(1,i+1)=10 +
deltaA(1,i+1)+aa;end%=========== ============Phi=[1 T -0.5*T^2; 0 1
-T;0 0 1]; % Ksi=[0.5*(T^2) ; T ; 0]; % Ge = -Ksi; % H=[1 0 0]; %
R= sigmaz^2;% x00=[0;0;0] ; P00=[0 0 0;0 0 0;0 0 0.0025];P=
cell(n/T,n/T);x= cell(n/T,n/T); % %'D:\ki IX\ICTI\kur
\result11.doc'=> % %
1=============================================P10=Phi*P00*transpose(Phi)+Ge*(sigma_delta_a^2)*transpose(Ge);A1=P10*transpose(H)*((H*P10*transpose(H)+R)^(-1));Y(1,1)=((YY(1,2)+Z(1,2))-B*(YY(1,1)+Z(1,1)))/E;x10=Phi*x00
+ Ksi*a(1,1);x{1,1}=x10 + A1*(Y(1,1)-
H*x10);P{1,1}=(eye(3,3)-A1*H)*P10; %C 2-
=====================================for i
=2:1:n/TP{i,i-1}=Phi*P{i-1,i-1}*transpose(Phi)+Ge*(0.05^2)*transpose(Ge);Ai=P{i,i-1}*transpose(H)*((H*P{i,i-1}*transpose(H)+R)^(-1));Y(1,i)=((YY(1,i+1)+Z(1,i+1))-B*(YY(1,i)+Z(1,i)))/E;x{i,i-1}=Phi*x{i-1,i-1}
+ Ksi*a(1,i);x{i,i}=x{i,i-1} + Ai*(Y(1,i)-
H*x{i,i-1});P{i,i}=(eye(3,3)-Ai*H)*P{i,i-1};endmore off; result_id
= fopen('E:\hoc tap\diplom\hoan thanh\diplom\result4.doc','w');
fprintf(result_id,' variant : T = %1.4f , sigmaz = %3d m C = %1.1f
\n',[T,sigmaz,C]); fprintf(result_id,'t(s)| H_d(m)| H_op(m)|
sigma_H| delta_H \n'); fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %6.4f| %6.4f| %5.4f|
%6.4f\n',[i*T ; Hg(1,i);x{i,i}(1,1) ;
sqrt(P{i,i}(1,1));Hg(1,i)-x{i,i}(1,1)]);
A(1,i*T)=sqrt(P{i,i}(1,1));end fprintf(result_id,' \n');
fprintf(result_id,'t(s)| V(m/s)| V_op(m/s)| sigma_V| delta_V \n');
fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %6.4f| %6.4f| %5.4f| %6.4f
\n',[i*T ; V(1,i);x{i,i}(2,1) ;
sqrt(P{i,i}(2,2));V(1,i)-x{i,i}(2,1)]);
BB(1,i*T)=sqrt(P{i,i}(2,2));end fprintf(result_id,'t(s)| ^a(m/s)|
^a_op(m/s)| sigma_^a| delta_^a \n'); fprintf(result_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(result_id,'%3d| %1.4f| %6.4f| %5.4f| %6.4f
\n',[i*T ; aa ;x{i,i}(3,1) ; sqrt(P{i,i}(3,3));aa-x{i,i}(3,1)]);
C(1,i*T)=sqrt(P{i,i}(3,3));end fprintf(result_id,'
\n');fclose(result_id);% ,% figure;clf;t=1:n;plot(t,A,'r-*');grid
on;hold on;plot(t,BB,'k--');plot(t,C,'b-+');xlabel('
t()');ylabel('CKO_\Delta_H() ,
CKO_\Delta_V(/),CKO_\Delta_\Delta_a(/2)
');legend('CKO_\Delta_H','CKO_\Delta_V','CKO_\Delta_\Delta_a');title('
. . \Deltaa');
%==========================================================================for
i=1:1:n/THop(1,i)=x{i,i}(1,1);Vop(1,i)=x{i,i}(2,1);end; %
==================================figure;i=1:1:n/T;plot(i*T,VV-Vop,'r-');grid
on;xlabel(' t()');ylabel(' (/)');legend(' ');title(' '); %
================================figure;i=1:n/T;plot(i*T,HH-Hop,'r-','LineWidth',2);hold
on;grid on;xlabel(' t()');ylabel(' ()');legend(' ');title(' ');
6.2.2. .%
.%==========================================================================
clc;clear;n=45% T=0.01; % g=10; % a=10;aa=0.1; % C=0.05; %
k=0:1:(n/T);t=k*T;%
==============================================Hg =
5*g*t/(0.2*pi)-5*g*(sin(0.2*pi*t))/(0.2*pi)^2; V
=5*g/(0.2*pi)-5*g*cos(0.2*pi*t)/(0.2*pi); for i=1:1:n/T
HH(1,i)=Hg(1,i);
VV(1,i)=V(1,i);end;E=1-exp(-T/C);B=exp(-T/C);%===============YY(k)======================================================sigmaz=5;
Z= sigmaz*randn(1,n/T);
k1=0:1:n/T+1;t1=k1*T;I1=t1*C-C^2+C^2*exp(-t1/C);I2=(0.2*pi*exp(-1/C*t1)+1/C*sin(0.2*pi*t1)-0.2*pi*cos(0.2*pi*t1))/(1/C^2+(0.2*pi)^2);YY=
5*g*I1/(0.2*pi*C) -5*g*I2/((0.2*pi)^2*C);%==========
==============================sigma_delta_a=0.05 ;% deltaA =
sigma_delta_a*randn(1,n/T+1); for i =0:1:(n/T)
a(1,i+1)=5*g*sin(0.2*pi*(i+1)*T) + deltaA(1,i+1)+ aa;end
%=========== ====================================Phi=[1 T -0.5*T^2;
0 1 -T;0 0 1]; % Ksi=[0.5*(T^2) ; T ; 0]; % Ge = -Ksi; % H =[1 0
0]; % % x00=[0;0;0] ; P00=[0 0 0;0 0 0;0 0 0.0025];P=
cell(n/T,n/T);x= cell(n/T,n/T); % , %
1=============================================P10=Phi*P00*transpose(Phi)+Ge*(sigma_delta_a^2)*transpose(Ge);A1=P10*transpose(H)*((H*P10*transpose(H)+sigmaz^2)^(-1));Y(1,1)=(YY(1,3)-B*YY(1,1))/E+Z(1,2);x10=Phi*x00
+ Ksi*a(1,1);x{1,1}=x10 + A1*(Y(1,1)-
H*x10);P{1,1}=(eye(3,3)-A1*H)*P10; %C 2-
=====================================for i
=2:1:n/TP{i,i-1}=Phi*P{i-1,i-1}*transpose(Phi)+Ge*(0.05^2)*transpose(Ge);Ai=P{i,i-1}*transpose(H)*((H*P{i,i-1}*transpose(H)+sigmaz^2)^(-1));Y(1,i)=(YY(1,i+2)-B*YY(1,i+1))/E+Z(1,i);x{i,i-1}=Phi*x{i-1,i-1}
+ Ksi*a(1,i);x{i,i}=x{i,i-1} + Ai*(Y(1,i)-
H*x{i,i-1});P{i,i}=(eye(3,3)-Ai*H)*P{i,i-1};end resultsin107_id =
fopen('E:\hoc tap\diplom\hoan thanh\diplom\result5.doc','w'); %
%'E:\hoc tap\diplom\hoan thanh\diplom\result5.doc'=> %
fprintf(resultsin107_id,' variant : T = %1.4f , sigmaz = %3d m C =
%1.1f \n',[T,sigmaz,C]); fprintf(resultsin107_id,'t(s)| H_d(m)|
H_op(m)| sigma_H| delta_H \n'); fprintf(resultsin107_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(resultsin107_id,'%3d| %3.3f| %6.3f| %5.3f|
%6.3f\n',[i*T ; Hg(1,i);x{i,i}(1,1) ;
sqrt(P{i,i}(1,1));Hg(1,i)-x{i,i}(1,1)]);
A(1,i*T)=sqrt(P{i,i}(1,1));end fprintf(resultsin107_id,' \n');
fprintf(resultsin107_id,'t(s)| V(m/s)| V_op(m/s)| sigma_V| delta_V
\n'); fprintf(resultsin107_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(resultsin107_id,'%3d| %3.3f| %6.3f| %5.3f|
%6.3f \n',[i*T ; V(1,i);x{i,i}(2,1) ;
sqrt(P{i,i}(2,2));V(1,i)-x{i,i}(2,1)]);
BB(1,i*T)=sqrt(P{i,i}(2,2));end fprintf(resultsin107_id,'t(s)|
^a(m/s)| ^a_op(m/s)| sigma_^a| delta_^a \n');
fprintf(resultsin107_id,'
------------------------------------------------------- \n'); for
i=1/T:1/T:n/T fprintf(resultsin107_id,'%3d| %1.3f| %6.3f| %5.3f|
%6.3f \n',[i*T ; aa ;x{i,i}(3,1) ;
sqrt(P{i,i}(3,3));aa-x{i,i}(3,1)]); C(1,i*T)=sqrt(P{i,i}(3,3));end
fprintf(resultsin107_id,' \n');fclose(resultsin107_id);
figure;clf;t=1:n;plot(t,A,'r-*');grid on;hold
on;plot(t,BB,'k-');plot(t,C,'b-+');xlabel('
t()');ylabel('CKO_\Delta_H() ,
CKO_\Delta_V(/),CKO_\Delta_\Delta_a(/2)
');legend('CKO_\Delta_H','CKO_\Delta_V','CKO_\Delta_\Delta_a');title('
. . \Deltaa');
%==========================================================================
for i=1:1:n/THop(1,i)=x{i,i}(1,1);Vop(1,i)=x{i,i}(2,1);end;%
==================================figure;i=1:n/T;plot(i*T,HH-Hop,'r-','LineWidth',1);hold
on;grid on;xlabel(' t()');ylabel(' ()');legend(' ');title(' '); %
================================figure;i=1:1:n/T;plot(i*T,VV-Vop,'r-');grid
on;xlabel(' t()');ylabel(' (/)');legend(' ');title(' ');
63
![Kalman Filter Algorithm · [๑] Kalman Filter ถูกนํามาใช เป นครั้งแรกเพ ื่อประมาณสถานะของระบบน](https://img.pdfslide.tips/doc/110x75/6062223c123db0056e485b97/kalman-filter-a-kalman-filter-aaaaaaaaafa-aa-aaaaaaaaaaa.jpg)
