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

Question

Hieu Le 2020-5-8

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/524052-discretize-stae-space-model-for-kalman-filter-runge-kutta-4th-order

评论： Hieu Le 2020-5-9

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 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Sulaymon Eshkabilov 2020-5-9

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/524052-discretize-stae-space-model-for-kalman-filter-runge-kutta-4th-order#answer_431311

在 MATLAB Online 中打开

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