discretize stae space model for Kalman filter - Runge-Kutta 4th order

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);
end
figure(1);clf(1);
plot(t,x3(i))

Answer 1

[Sulaymon Eshkabilov]

Check your equation that has some sign (-,+) problem (s).

Here is a much simpler code:

Vin=10;
w=2*pi;
Ts=w/(2*pi*2000);
t=0:Ts:10;
x(:,1)=0;
x(:,2)=0;
x(:,3)=0;
for ii=1:length(t)-1
    K1=Dummy(t(ii), x(ii,1), x(ii,2), x(ii,3));
    K2=Dummy(t(ii), x(ii,1)+K1(:,1)*Ts/2, x(ii,2)+K1(:,2)*Ts/2, x(ii,3)+K1(:,3)*Ts/2);
    K3=Dummy(t(ii), x(ii,1)+K2(:,1)*Ts/2,x(ii,2)+K2(:,2)*Ts/2,x(ii, 3)+K2(:,3)*Ts/2);
    K4=Dummy(t(ii), x(ii,1)+K3(:,1)*Ts,x(ii,2)+K3(:,2)*Ts,x(ii, 3)+K3(:,3)*Ts);
    x(ii+1,:) =  x(ii,:)+Ts/6*(K1+2*K2+2*K3+K4);
end
figure(1);clf(1);
plot(t,x(:,3))
function F = Dummy(t, x1, x2, x3)
R1=5;
R2=6;
L1=8e-6;
L2=9e-6;
Cap=5e-9;
w=2*pi;
F=[((1/L1)*(10*sin(w*t)-R1*x1-x3)),...
    ((1/L2)*(x3-R2*x2)), ((1/Cap)*(-x1-x2))];
end

[Hieu Le]

Thank you for your help. Your code is much more simpler, but it still doesn't work.

[Sulaymon Eshkabilov]

You'd need to check the signs in your equation AND parameters L1, L2, Cap. Once you correct them, then edit the proposed code respectively. As it is the above code is correct with respect to your ODEs and ICs. E.g.: if you change the values of L1, L2, Cap, then the system behavoir becomes more reasonable.

Good luck.

[Hieu Le]

Can you explain it more clearly? I didn't catch your idea.

[Sulaymon Eshkabilov]

1) SIGNS: dx1 = (1/L1)(u ??? R*x1); dx2 = ??? (1/L2)*( ??? x3 ??? R3*x2); ...

2) VALUES: L1=8; L2=9;Cap=5; % These values give you more reasonable numerical solutions

[Hieu Le]

About signs, 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);

are there any signs that confuse you?

About values. I think L1=8(H) is really large over than commmon.

[Sulaymon Eshkabilov]

What I am trying to say is maybe your handwritten equation is not correctly taken out from the source.

Good luck.

[Hieu Le]

Thank you. I'll reconsider it.