- Kalman Filter in Matlab (Tutorial) - File Exchange - MATLAB Central
- teasit/matlab-kalman-filters: Kalman Filter implementations written in MATLAB with code-generation capabilities
Simulink Kalman Filter Function for 2nd Order Systems
12 views (last 30 days)
Show older comments
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;
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1150285/image.png)
Undermask for my 2nd order subsystem
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1150290/image.png)
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;
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1150295/image.png)
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.)
0 Comments
Answers (1)
Shivam Malviya
on 14 Oct 2022
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;
If you are interested in implementing it from scratch, please check out the following;
If you are fine with using the inbuilt functions and blocks, these links might help;
I hope this helps!
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!