Revision 8, 0.8 kB
(checked in by smidl, 16 years ago)
|
Kalmany funkci, PF nefunkci
|
-
Property svn:eol-style set to
native
-
Property svn:keywords set to
Rev Author Date
|
Rev | Line | |
---|
[7] | 1 | if 0 |
---|
| 2 | A=[1 -0.5; 1 0]; |
---|
| 3 | B=[1;0]; |
---|
| 4 | C=[1 0]; |
---|
| 5 | D=[0]; |
---|
| 6 | R=[0.1]; |
---|
| 7 | Q=[0.2 0 ; 0 0.02]; |
---|
| 8 | |
---|
| 9 | sQ = chol(Q)'; |
---|
| 10 | sR = chol(R)'; |
---|
| 11 | |
---|
| 12 | N =1000; |
---|
[8] | 13 | mu0 = [0;0]; |
---|
| 14 | P0 = 200*eye(2); |
---|
[7] | 15 | |
---|
| 16 | u = ones(1,N); |
---|
| 17 | x = zeros(2,N); |
---|
| 18 | y = zeros(1,N); |
---|
| 19 | |
---|
| 20 | x(:,1) = [10;10]; |
---|
| 21 | for i=2:N; |
---|
| 22 | et(1:2,i) = sQ*randn(2,1); |
---|
| 23 | x(:,i) = A*x(:,i-1) + B*u(i) + et(:,i); |
---|
| 24 | y(:,i) = C*x(:,i) + D*u(i) + sR*randn(1,1); |
---|
| 25 | end |
---|
| 26 | |
---|
| 27 | d=[y;u]; |
---|
[8] | 28 | itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0) |
---|
| 29 | save testKF |
---|
[7] | 30 | else |
---|
| 31 | %!cd ../testKF |
---|
| 32 | |
---|
[8] | 33 | load testKF |
---|
[7] | 34 | % init |
---|
[8] | 35 | mu = mu0; |
---|
| 36 | P = P0; |
---|
| 37 | EP = [0;0]; |
---|
[7] | 38 | |
---|
| 39 | for t=2:N |
---|
| 40 | mu = A*mu + B*u(t); |
---|
| 41 | P = A*P*A' + Q; |
---|
| 42 | |
---|
| 43 | %Data update |
---|
| 44 | Ry = C*P*C' + R; |
---|
| 45 | iRy = inv(Ry); |
---|
| 46 | K = P*C'*iRy; |
---|
| 47 | P = P- K*C*P; % P = P -KCP; |
---|
[8] | 48 | EP(:,t) = eig(P); |
---|
| 49 | if any(EP<0), keyboard; end |
---|
[7] | 50 | mu = mu + K*(y(t)-C*mu-D*u(t)); |
---|
| 51 | Mu(1:2,t)=mu; |
---|
| 52 | end |
---|
| 53 | |
---|
| 54 | |
---|
| 55 | |
---|
| 56 | itload('testKF_res.it'); |
---|
| 57 | |
---|
| 58 | hold off |
---|
| 59 | plot(x'); |
---|
| 60 | hold on |
---|
| 61 | plot(xth','--'); |
---|
[8] | 62 | plot(xth2','+'); |
---|
[7] | 63 | plot(Mu',':'); |
---|
| 64 | end |
---|