How to use Kalman Filter for finding displacement on 2nd order mass spring damper system

조회 수: 15(최근 30일)
Arda Canbas
Arda Canbas 2022년 1월 16일
댓글: Elad Kivelevitch 2022년 1월 24일
I made a mass spring damper system and gave a input force to one of the masses than tried to find the second masses displacement after force applied and tried to use kalman filter to do that but i didnt worked out. How can i apply kalman filter to my system? There is the code,
syms x1(t) x2(t) x3(t) x4(t) f2
m1=65; % first mass
m2=20; % second mass
b1=600; % damping of the first damper
b2=800; % damping of the second damper
k1=1500; % stiffness of the spring
k2=2000; % stiffness of the second spring
k3=5000; % stiffness of the third spring
f2 = sin(9*t); % sinusoidal impulse input on the first mass
X = [x1(t); x2(t); x3(t); x4(t)]; %column vector of the states where x1(t) is the position of the m1 x2(t) is the position of the m2
%x3(t) is the velocity of the first m1 and x4(t) is the velocity of the m2
A = [0 0 1 0; 0 0 0 1;-(k1+k2+k3)/m1 k1/m1 -(b1+b2)/m1 b2/m1;k1/m2 -k1/m2 b2/m2 -b2/m2]; %the system matrix for how the system will behave
B = [0; 0; 0; 1/m1]; %input matrix for how the input affects the system
eqns = diff(X) == A*X + f2*B; %setting the differential system to a variable
x1 =2; %initial position of the m1
x2 =3; %initial position of the m2
x3 =6; %initial velocity of the m1
x4 =7; %initial velocity of the m2
conds = [cond1;cond2;cond3;cond4]; %the conditional column vector
initialState = [x1;0;x2;0];
KF = trackingKF(A,B,initialState);


Pratyush Roy
Pratyush Roy 2022년 1월 19일
Hi Arda,
The function trackingKF can be used for tracking using a linear Kalman Filter.
The two-body mass spring system equations can be mapped to discrete-state equations in the following manner:
= +
Keeping terms associated with k+1 to the left and shifting everything else to the right we obtain:
= +
By comparing with the equation x[k+1] = F[k]x[k]+G[k]u[k]+v[k], we obtain that,
F[k] =
G[k] =
We can also set the displacements as the elements of the measurement vector. In that case the matrix equation becomes:
By comparing this with the equation z[k] = H[k]x[k] + w[k], we obtain:
H[k] =
Now these vectors F,G,H can be passed to the trackingKF function along with the initial state:
filter = trackingKF(F,H,G,'State',InitialState);
After obtaining the filter object, we can use different object functions (e.g. the predict function) for different applications.
Please refer to the documentation links below for more details:
Hope this helps!
  댓글 수: 1
Elad Kivelevitch
Elad Kivelevitch 2022년 1월 24일
A couple of notes on the above answer:
  1. In the derivation of the force input, it seems that an assumption of a "nearly constant" force was used. This may be OK if T is very very small, but that requires validation. In any case, sin(9t) seems to be the forcing function, therefore G(k) should be different and sin(9t)/m1 should be considered as the input u.
  2. Note that in the call to predict, you will need to provide the input force.
  3. While you should be able to continue using trackingKF for this purpose, I think that it would be easier to use extendedKalmanFilter, and define the state transition function and measurement function. In the state transition function you will be able to control the input force more easily, in my opinion.

댓글을 달려면 로그인하십시오.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by