discretize stae space model for Kalman filter - Runge-Kutta 4th order
9 vues (au cours des 30 derniers jours)
Afficher commentaires plus anciens
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 commentaires
Réponse acceptée
Sulaymon Eshkabilov
le 9 Mai 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 commentaires
Sulaymon Eshkabilov
le 9 Mai 2020
What I am trying to say is maybe your handwritten equation is not correctly taken out from the source.
Good luck.
Plus de réponses (0)
Voir également
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!