Simulink Kalman Filter Function for 2nd Order Systems

In order to better understand the Kalman filter, I set up a 2nd order mass spring damper system by myself and tried to filter it by adding random noise. I've been trying for a few days but I couldn't fix the signal somehow. I want to keep the code as clear and simple as possible, where do you think there is a mistake?

m=5;
b=3;
k=2;

Undermask for my 2nd order subsystem

My simulink system

where Q (for process noise ) = 1e-3 and R (for measure noise) =1e-4 (means are zero)

function xhatout  = fcn(u,y)

%define system variable
m=5;
b=3;
k=2;

%define state space vectors
A=[0 1; -k/m -b/m]; %transition
B=[0 ; 1/m]; %input
C=[0 1]; %measurement
x=[0;0];xhat=x; %initial state

%Noise
Q=(1e-3); %Process Noise
R=(1e-4); %Measurement Noise

%Noise Covariance
Sz = (R^2); %Measurement Noise
Sw = B*(Q^2)*B'; %process noise cov
P=Sw;%initial estimate covariance

%Noise added to formulas 
ProcessNoise=Q*(B*randn);
x=A*x+B*u+ProcessNoise;

MeasNoise=R*randn;
y=C*x+MeasNoise;

%Kalman Formula
xhat=A*xhat + B*u;

%Innovation Matrix
resid=y-C*xhat;

s=C*P*C'+Sz;

%Kalman Gain
K=A*P*C'*inv(s);

xhat=xhat+K*resid;

%Noise Error Covariance
P=A*P*A'+Sw-A*P*C'*inv(s)*C*P*A';

xhatout=xhat;

My source is Kalman Filter Dan Simon article. And for this code I got this output;

as i have tried many things there is some big error because output is not changing against my minor modifications.

(I'm sorry if there are any mistakes in the way I use the forum.)

Answer 1

[Shivam Malviya]

Hi Emir,

I understand that you want to understand the Kalman filter better, and for that, you are implementing it. But it is not giving the expected output.

I encourage you to explore the following link to understand the Kalman filter better;

• Understanding Kalman Filters - MATLAB

If you are interested in implementing it from scratch, please check out the following;

• Kalman Filter in Matlab (Tutorial) - File Exchange - MATLAB Central
• teasit/matlab-kalman-filters: Kalman Filter implementations written in MATLAB with code-generation capabilities

If you are fine with using the inbuilt functions and blocks, these links might help;

• Kalman Filtering - MATLAB & Simulink
• How to Implement a Kalman Filter in Simulink

I hope this helps!

[Emir Kulakoglu]

I've already read them all. But thank you anyway!!. I determined some structure mistakes.