-
Notifications
You must be signed in to change notification settings - Fork 25
/
particleFilterExample2.m
32 lines (27 loc) · 1.1 KB
/
particleFilterExample2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
clc; clear; close all;
file = fullfile('..','data','ParticleFilterDataRaw.csv');
X = dlmread(file,';',1,0);
t = X(:, 1)';
y = X(:, 2)';
% Do particle filtering - Tuning parameters
p = 14; % Length of the horizon (Change this)
% Particle filter - No tuning
[m, n] = size(y); % Dimension of the output state and length n
yf = zeros(m, n); % Filtered outputs
horizon = zeros(m, p); % Horizon matrix
xhatp = zeros(m, 1); % Past estimated state
xhatp(1) = y(1); % First estimation is a real measurement
k = 1; % Horizon counting (will be counted to p. Do not change this)
noise = rand(m, p); % Random noise, not normal distributed
% Particle filter - Simulation
for i = 1:n
x = y(:, i); % Get the state
[xhat, horizon, k, noise] = mi.pf(x, xhatp, k, horizon, noise);
yf(:, i) = xhat; % Estimated state
xhatp = xhat; % This is the past estimated state
end
% Plot restult
plot(t, y)
hold on
plot(t, yf, '-r')
grid on