discretize stae space model for Kalman filter - Runge-Kutta 4th order
9 visualizzazioni (ultimi 30 giorni)
Mostra commenti meno recenti
Hieu Le
il 8 Mag 2020
Commentato: Hieu Le
il 9 Mag 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 Commenti
Risposta accettata
Sulaymon Eshkabilov
il 9 Mag 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 Commenti
Sulaymon Eshkabilov
il 9 Mag 2020
What I am trying to say is maybe your handwritten equation is not correctly taken out from the source.
Good luck.
Più risposte (0)
Vedere anche
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!