-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathupdatedkalmar.m
152 lines (148 loc) · 5.24 KB
/
updatedkalmar.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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
clc;
clear all;
close all;
%% Video Initialization
video_name = 'file_example_AVI_480_750kB.avi'; %Video name
vid = VideoReader(video_name);
nframes = vid.NumberOfFrames; %Number of frames
Height = vid.Height; % Height :)
Width = vid.Width; % Width :)
thr = 10; % Threshold for generating binary image of the noise
%% Kalman Filter Definition
% First, we define the state of interest. In this case, we define the
% following variables for our states: state(t) = [X Y dx dy (d^2)x (d^2)y](t)
% X(t+1) = 1/2(a)T^2 + V(t)T + X(t); where a and V denotes the acceleration
% and velocity respectively.
% V(t+1) = aT + V(t)
% a(t+1) = a(t) ; assuming constant acceleration
%State(t+1) = A.State(t) + B.u + <State Uncertainty|State Noise>
dt=0.1;
% A = [1 0 dt 0 (dt^2)/2 0;
% 0 1 0 dt 0 (dt^2)/2;
% 0 0 1 0 dt 0;
% 0 0 0 1 0 dt;
% 0 0 0 0 1 0;
% 0 0 0 0 0 1];
A = csvread('a.csv' );
B = csvread('b.csv' );
%B = [(dt^2)/2 (dt^2)/2 dt dt 1 1]';
% B=0;
% Input to the system (here acceleration).
u = 4e-3;
%We are observing the X, and Y position. So our observation is: y = [X Y]
%y(t) = H.State(t) + <Measurement Noise>
% H = [1 0 0 0 0 0
% 0 1 0 0 0 0];
H = [csvread('x.csv' );
csvread('y.csv' )];
%%% Covariance Matrices :
%I) Dynamic Noise, II) Measurement Noise, III) State Variables
% Now we define the state uncertainty; or, the state covariance matrix -> S(t)
% For simplicity we are assuming that the state variables are independet.
% The state variables are independet, so the covariance matrix is a diagonal matrix.
State_Uncertainty = 100;
S = transpose(State_Uncertainty * eye(size(A,1))); % The state variables are independet, so the covariance matrix is a diagonal matrix.
% Defining the <Measurement Noise> Covariance Matrix R
Meas_Unertainty = 1;
R = [0.25 0;
0 0.25];
% Defining the Dynamic Noise covariance matrix
% Dynamic noise characterize the transition noise from one state to another
Dyn_Noise_Variance = (0.01)^2;
%Shahin = [(1/2)*(dt^2) (1/2)*(dt^2) dt dt 1 1]'; %Constant Acceleration
% Shahin = [(1/2)*(dt^2) (1/2)*(dt^2) dt dt]'; %Constant Velocity
%Q = Shahin*Shahin'*Dyn_Noise_Variance;
% Assuming the variables X and Y are independent
Q = [0.16 0 0 0;
0 0.36 0 0;
0 0 0.16 0;
0 0 0 0.36
];
%% Kalman Variables (By the way I do not like Kalman filter, it has been overused)
Input = [];
x = [];
Kalman_Output = [];
% x = [Height/2; Width/2; 0; 0;0;-9.8]; % Initial Values
x = csvread('x.csv' ); % Initial Values
%% Extracting Background
background_frame = BackgroundExt(video_name);
%% Extract the noise
moving = zeros(Height,Width,nframes);
labeled_frames = zeros(Height,Width,nframes);
bb=0;
for i=1:nframes-1
current_frame = double(read(vid,i));
moving(:,:,i) = (abs(current_frame(:,:,1) - background_frame(:,:,1)) > thr)...
|(abs(current_frame(:,:,2) - background_frame(:,:,2)) > thr)...
|(abs(current_frame(:,:,3) - background_frame(:,:,3)) > thr);
moving(:,:,i) = bwmorph(moving(:,:,i),'erode',2);
labeled_frames(:,:,i) = bwlabel(moving(:,:,i),4);
stats{i} = regionprops(labeled_frames(:,:,i),'basic');
[n_obj,features] = size(stats{i});
area = 0;
if(n_obj ~= 0)
for k=1:n_obj
if(stats{i}(k).Area > area)
id(i) = k;
area = stats{i}(k).Area;
end
end
centroid(:,:,i) = stats{i}(id(i)).Centroid;
else
centroid(:,:,i) = [rand*200 rand*200];
bb = bb+1;
end
i %indicates the frame number
end
%% Marking Noise
for r=1:nframes-1
frames = read(vid,r);
frames = insertShape(frames,'circle',[centroid(1,1,r) centroid(1,2,r) sqrt(stats{r}(id(r)).Area/pi)],'LineWidth',1);
marked_noise(:,:,:,r) = frames;
%imshow(frames);
end
for r=1:nframes-1
frames = read(vid,r);
frames = insertShape(frames,'circle',[centroid(1,1,r) centroid(1,2,r) 4],'LineWidth',2);
%%Kalman Update
% Original Tracker.
if(mod(r,2) == 0)
input = [centroid(1,1,r); centroid(1,2,r)];
else
input=[];
end
% if(r>50)
% input=[];
% else
% input = [centroid(1,1,r); centroid(1,2,r)];
% frames = insertShape(frames,'circle',[centroid(1,1,r) centroid(1,2,r) 4],'LineWidth',2);
% end
% Estimate the next state
x = A*transpose(x) + B*u;
% Estimate the error covariance
S = A*S*A' + Q;
% Kalman Gain Calculations
K = S*H'*inv(H*S*H'+R);
% Update the estimation
if(~isempty(input)) %Check if we have an input
x = x + K*(input - H*x);
end
% Update the error covariance
S = (eye(size(S,1)) - K*H)*S;
% Save the measurements for plotting
Kalman_Output = H*x;
frames = insertShape(frames,'circle',[Kalman_Output(1) Kalman_Output(2) 4],'LineWidth',2,'Color','black');
scenario_1(:,:,:,r) = frames;
%imshow(frames)
r
end
%% Showing the Video
implay(read(vid));
%% Showing Background
imshow(uint8(background_frame));
%% Extracted Noise Black and White
implay(moving);
%% Marked Noise - Original Tracker
implay(marked_noise(:,:,:,:));
%% Scenario 1 - Not Enough Samples
implay(scenario_1,15);