Revision 1105, 0.9 kB
(checked in by zimamiro, 14 years ago)
|
|
Rev | Line | |
---|
[1105] | 1 | function [ztrata u y b P]=rizeni(H,ny,system,apriori) |
---|
| 2 | |
---|
| 3 | sigma=system.sigma; |
---|
| 4 | N=system.horizont; |
---|
| 5 | %%chce to mzslet na PYB |
---|
| 6 | |
---|
| 7 | y=zeros(1,N); |
---|
| 8 | b=zeros(1,N); |
---|
| 9 | P=zeros(1,N); |
---|
| 10 | u=zeros(1,N-1); |
---|
| 11 | ztrata=0; |
---|
| 12 | |
---|
| 13 | y(1)=apriori(1); |
---|
| 14 | b(1)=apriori(2); |
---|
| 15 | P(1)=apriori(3); |
---|
| 16 | |
---|
| 17 | |
---|
| 18 | for i=1:size(H,1) |
---|
| 19 | eta=abs(y(i))/sigma; |
---|
| 20 | beta=abs(b(i))/sqrt(P(i)); |
---|
| 21 | state=[eta beta]; |
---|
| 22 | |
---|
| 23 | index=find_in_hyperstate5(state, H(i,:,:)); |
---|
| 24 | |
---|
| 25 | % plot(H(i,:,1),H(i,:,2),'g.'); hold on; plot(H(i,index,1), H(i,index,2),'r+'); plot(state(1), state(2),'b.'); hold off |
---|
| 26 | |
---|
| 27 | u(i)=-ny(i,index)*sigma/sqrt(P(i))*sign(y(i)*b(i)); |
---|
| 28 | |
---|
| 29 | y(i+1)=y(i)+system.b*u(i)+sigma*randn; |
---|
| 30 | K=u(i)*P(i)/(P(i)*u(i)^2+sigma^2); |
---|
| 31 | b(i+1)=b(i)+K*(y(i+1)-y(i)-b(i)*u(i)); |
---|
| 32 | P(i+1)=(1-K*u(i))*P(i); |
---|
| 33 | |
---|
| 34 | ztrata=ztrata+(y(i+1)-system.yr(i))^2; |
---|
| 35 | end |
---|
| 36 | end |
---|