-
Notifications
You must be signed in to change notification settings - Fork 0
/
Update_param.m
52 lines (34 loc) · 1.35 KB
/
Update_param.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
function Fa = Update_param( param,Xf,Ff,PXf,PFf,CL2,H,Yp,R )
Fa= zeros( param.Np,param.Ne );
z = zeros( 1,param.Ne );
if ( strcmp(param.us,'batch') == 1 )
[ U,S,V ]= svd( CL2 .* ( H*PXf*H' ) + R ) ;
Ti = find( cumsum( diag(S)/sum(S(:)) ) > 0.98,1 );
Im = V( :,1:Ti )*diag( 1./diag( S(1:Ti,1:Ti) ) )*U( :,1:Ti )';
Gain = ( PFf*H' ) * Im ;
for e = 1:param.Ne
Fa( :,e )= Ff( :,e ) + Gain * ( Yp( :,e ) - H*Xf( :,e ) ) ;
end
elseif ( strcmp(param.us,'ser') == 1 )
Fa= Ff; Fam= mean( Fa,2 );
Xa= Xf; Xam= mean( Xa,2 );
for ip= 1:param.No
zm= H(ip,:)*Xam;
for ie= 1:param.Ne
z(ie)= H(ip,:)*Xa(:,ie);
end
sumt= 0; sumo= 0;
for ie= 1:param.Ne
sumt= sumt+( Fa(:,ie)-Fam )*( z(ie)-zm );
sumo= sumo+( z(ie)-zm )^2;
end
sumo= sumo+(param.Ne-1)*R(ip,ip);
for ie= 1:param.Ne
Fa(:,ie)= Fa( :,ie ) + sumt/sumo*( Yp(ip,ie)-z(ie) );
end
Fam= mean( Fa,2 );
end
else
warning('Update_param: "Update Scheme" is not defined, analysis is ignored.')
end
end