I'm working on designing an Extended Kalman Filter for a non-linear system, that's needed to be discreize first.
I intend to use Runge-Kutta method to do it, hence I tried an easy exam like this code. But it went to infinite. I tried many ways but I couldn't find any mistakes.
Can anyone give me some suggest, please!!!!
%% Params:
R1=5;R2=6;L1=8e-6;L2=9e-6;Cap=5e-9;Vin=10;w=2*pi;Ts=w/(2*pi*2000);%% Code
f1=@(x1,x3,u) 1/L1*(u-R1*x1-x3); f2=@(x2,x3) 1/L2*(x3-R2*x2); f3=@(x1,x2) 1/Cap*(x1-x2);t=0:Ts:10;u=10*sin(w*t);x1=zeros(size(t));x2=zeros(size(t));x3=zeros(size(t));for i=1:length(t)-1 K1=f1(x1(i),x3(i),u(i)); K2=f1(x1(i)+K1*Ts/2,x3(i)+K1*Ts/2,u(i)+K1*Ts/2); K3=f1(x1(i)+K2*Ts/2,x3(i)+K2*Ts/2,u(i)+K2*Ts/2); K4=f1(x1(i)+K3*Ts,x3(i)+K3*Ts,u(i)+K3*Ts); x1(i+1)=x1(i)+Ts/6*(K1+2*K2+2*K3+K4); K1=f2(x2(i),x3(i)); K2=f2(x2(i)+K1*Ts/2,x3(i)+K1*Ts/2); K3=f2(x2(i)+K2*Ts/2,x3(i)+K2*Ts/2); K4=f2(x1(i)+K3*Ts,x3(i)+K3*Ts); x2(i+1)=x2(i)+Ts/6*(K1+2*K2+2*K3+K4); K1=f3(x1(i),x2(i)); K2=f3(x1(i)+K1*Ts/2,x2(i)+K1*Ts/2); K3=f3(x1(i)+K2*Ts/2,x2(i)+K2*Ts/2); K4=f3(x1(i)+K3*Ts,x2(i)+K3*Ts); x3(i+1)=x2(i)+Ts/6*(K1+2*K2+2*K3+K4);endfigure(1);clf(1);plot(t,x3(i))
Best Answer