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

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);
T=0.5;
pos=[0:x3*T:2;5:x4*T:6]';

Answer 1

[Pratyush Roy]

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:

https://in.mathworks.com/help/fusion/ref/trackingkf.html

https://in.mathworks.com/help/fusion/ug/linear-kalman-filters.html

Hope this helps!

[Elad Kivelevitch]

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.