0001 classdef KalmanFilt
0002 properties
0003 A
0004 B
0005 H
0006 R
0007 Q
0008 end
0009
0010 methods
0011 function kf = KalmanFilt()
0012
0013
0014
0015
0016
0017
0018 r = .6;
0019
0020
0021 kf.A = [1 0 1 0;...
0022 0 1 0 1;...
0023 0 0 1 0;...
0024 0 0 0 1];
0025
0026 kf.B = [0 0 0 0]';
0027
0028 kf.H = [1 0 0 0;...
0029 0 1 0 0;...
0030 0 0 1 0;...
0031 0 0 0 1];
0032
0033 kf.R = [1 0 1 0;...
0034 0 1 0 1;...
0035 0 0 1 0;...
0036 0 0 0 1];
0037
0038 kf.Q = r* [1 0 1 0;...
0039 0 1 0 1;...
0040 0 0 1 0;...
0041 0 0 0 1];
0042
0043 end
0044
0045 function track = predict(kf,track)
0046
0047
0048
0049
0050
0051
0052
0053 x = track.x;
0054 P = track.P;
0055
0056
0057
0058
0059
0060
0061
0062
0063 if strcmp(track.Type,'LOST')==1
0064
0065 s = sqrt(x(3).^2 + x(4).^2);
0066 theta = atan2(track.Cells(end).Type(2),track.Cells(end).Type(1));
0067 dx = s*cos(theta);
0068 dy = s*sin(theta);
0069
0070 track.x_ = [track.x(1)+dx; track.x(2)+dy; dx; dy];
0071 track.P_ = kf.A*P*kf.A' + kf.Q;
0072 else
0073 track.x_ = kf.A * x + kf.B;
0074 track.P_ = kf.A*P*kf.A' + kf.Q;
0075 end
0076
0077 if (track.x_(1) <0) || (track.x_(1) > 1000) || (track.x_(2) <0) || (track.x_(2) >1000)
0078 track.Type = 'INACTIVE';
0079 end
0080
0081
0082
0083 end
0084
0085 function track = update(kf,track)
0086
0087
0088
0089 if strcmp(track.Type,'ACTIVE')==1
0090 track.nKfCells = 0;
0091 x_ = track.x_;
0092 P_ = track.P_;
0093 cell = track.Cells(end);
0094
0095 cc = cell.Col;
0096 cr = cell.Row;
0097 pc = track.Cells(end-1).Col;
0098 pr = track.Cells(end-1).Row;
0099
0100 dc = cc - pc;
0101 dr = cr - pr;
0102
0103
0104
0105 z = [cc cr dc dr]';
0106
0107
0108 K = P_*kf.H'/(kf.H*P_*kf.H' + kf.R);
0109
0110 if (~isempty(z))
0111
0112 track.x = x_ + K*(z - kf.H*x_);
0113 end
0114
0115
0116 track.P = (eye(4) - K*kf.H)*P_;
0117
0118 track.Cells(end).Col = round(track.x(1));
0119 track.Cells(end).Row = round(track.x(2));
0120
0121 elseif (strcmp(track.Type,'LOST')==1)
0122 if (track.nKfCells < 3)
0123
0124 kfFrame = track.Cells(end).Frame + 1;
0125 if track.Cells(end).Id <100
0126 kfId = 100 + track.Cells(end).Id;
0127 else
0128 kfId = track.Cells(end).Id;
0129 end
0130 kfCol = track.x_(1);
0131 kfRow = track.x_(2);
0132 kfArea= track.Cells(end).Area;
0133 kfCell = WBC(kfFrame,kfId,kfRow,kfCol,kfArea,0);
0134 kfCell.Type = track.Cells(end).Type;
0135 track.add(kfCell);
0136 track.nKfCells = track.nKfCells + 1;
0137
0138
0139 x_ = track.x_;
0140 P_ = track.P_;
0141 cell = track.Cells(end);
0142
0143 cc = cell.Col;
0144 cr = cell.Row;
0145 pc = track.Cells(end-1).Col;
0146 pr = track.Cells(end-1).Row;
0147
0148 dc = cc - pc;
0149 dr = cr - pr;
0150
0151 z = [cc cr dc dr]';
0152
0153
0154 K = P_*kf.H'/(kf.H*P_*kf.H' + kf.R);
0155
0156 if (~isempty(z))
0157
0158 track.x = x_ + K*(z - kf.H*x_);
0159 end
0160
0161
0162 track.P = (eye(4) - K*kf.H)*P_;
0163
0164 track.Cells(end).Col = round(track.x(1));
0165 track.Cells(end).Row = round(track.x(2));
0166
0167 else
0168 track.Type = 'INACTIVE';
0169
0170 for ce = 1: track.nKfCells
0171 track.removeLast;
0172 end
0173 end
0174 elseif (strcmp(track.Type,'COLLIDE')==1)
0175 track.nKfCells = 0;
0176
0177
0178 track.x = [track.Cells(end).Col; track.Cells(end).Row; 0; 0];
0179
0180 end
0181
0182 cell = track.Cells(end);
0183 if cell.Row<0 || cell.Row>1000 || cell.Col<0 || cell.Col>1000
0184 track.Type='INACTIVE';
0185 end
0186 end
0187
0188 end
0189 end