Revision 33, 1.1 kB
(checked in by smidl, 16 years ago)
|
Oprava PF a MPF + jejich implementace pro pmsm system
|
-
Property svn:eol-style set to
native
-
Property svn:keywords set to
Rev Author Date
|
Rev | Line | |
---|
[9] | 1 | function testKF(skipgen) |
---|
[22] | 2 | if nargin<1, skipgen=0; end |
---|
[7] | 3 | |
---|
[9] | 4 | if ~skipgen |
---|
| 5 | A=[1 -0.5; 1 0]; |
---|
[33] | 6 | B=[1;0.1]; |
---|
| 7 | C=[1 0; 0 1]; |
---|
| 8 | D=[0.1; 0]; |
---|
| 9 | R=[1 0; 0 0.1]; |
---|
| 10 | Q=[0.2 0 ; 0 0.2]; |
---|
[7] | 11 | |
---|
[9] | 12 | sQ = chol(Q)'; |
---|
| 13 | sR = chol(R)'; |
---|
[7] | 14 | |
---|
[9] | 15 | N =1000; |
---|
| 16 | mu0 = [0;0]; |
---|
| 17 | P0 = 200*eye(2); |
---|
[7] | 18 | |
---|
[33] | 19 | u = rand(1,N); |
---|
[9] | 20 | x = zeros(2,N); |
---|
[33] | 21 | y = zeros(2,N); |
---|
[7] | 22 | |
---|
[9] | 23 | x(:,1) = [10;10]; |
---|
[32] | 24 | Et = sQ*randn(2,N); |
---|
[33] | 25 | Wt = sR*randn(2,N); |
---|
[9] | 26 | for i=2:N; |
---|
[32] | 27 | x(:,i) = A*x(:,i-1) + B*u(i) + Et(:,i); |
---|
| 28 | y(:,i) = C*x(:,i) + D*u(i) + Wt(:,1); |
---|
[9] | 29 | end |
---|
[7] | 30 | |
---|
[9] | 31 | d=[y;u]; |
---|
| 32 | itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0) |
---|
| 33 | save testKF |
---|
| 34 | else |
---|
| 35 | load testKF |
---|
| 36 | end |
---|
[7] | 37 | % init |
---|
[8] | 38 | mu = mu0; |
---|
| 39 | P = P0; |
---|
| 40 | EP = [0;0]; |
---|
[7] | 41 | |
---|
[32] | 42 | Oxt = mu0; |
---|
| 43 | OPt = P0; |
---|
| 44 | ll =0; |
---|
| 45 | Oxt2 = mu0; |
---|
| 46 | OPt2 = P0; |
---|
| 47 | ll2=0; |
---|
| 48 | |
---|
| 49 | |
---|
[7] | 50 | for t=2:N |
---|
| 51 | mu = A*mu + B*u(t); |
---|
| 52 | P = A*P*A' + Q; |
---|
| 53 | |
---|
| 54 | %Data update |
---|
| 55 | Ry = C*P*C' + R; |
---|
| 56 | iRy = inv(Ry); |
---|
| 57 | K = P*C'*iRy; |
---|
| 58 | P = P- K*C*P; % P = P -KCP; |
---|
| 59 | mu = mu + K*(y(t)-C*mu-D*u(t)); |
---|
| 60 | Mu(1:2,t)=mu; |
---|
[32] | 61 | |
---|
| 62 | [Oxt,OPt,ll(t)] = Kalman(Oxt,y(t),A,C,Q,R,OPt); |
---|
| 63 | [Oxt2,OPt2,ll2(t)] = Kalman(Oxt2,y(t),A,C,Q,R/10000,OPt2); |
---|
[7] | 64 | end |
---|
| 65 | |
---|
[32] | 66 | keyboard |
---|
| 67 | |
---|
[22] | 68 | !cd ../;./tests/testKF |
---|
[7] | 69 | itload('testKF_res.it'); |
---|
| 70 | |
---|
| 71 | hold off |
---|
| 72 | plot(x'); |
---|
| 73 | hold on |
---|
| 74 | plot(xth','--'); |
---|
[8] | 75 | plot(xth2','+'); |
---|
[22] | 76 | plot(xthE','o'); |
---|
[7] | 77 | plot(Mu',':'); |
---|
[33] | 78 | keyboard |
---|
[7] | 79 | end |
---|