discretize stae space model for Kalman filter - Runge-Kutta 4th order
9 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
Hieu Le
el 8 de Mayo de 2020
Comentada: Hieu Le
el 9 de Mayo de 2020
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))
0 comentarios
Respuesta aceptada
Sulaymon Eshkabilov
el 9 de Mayo de 2020
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
7 comentarios
Sulaymon Eshkabilov
el 9 de Mayo de 2020
What I am trying to say is maybe your handwritten equation is not correctly taken out from the source.
Good luck.
Más respuestas (0)
Ver también
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!