Home > track > KalmanFilt.m

KalmanFilt

PURPOSE ^

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 classdef KalmanFilt
0002     properties
0003         A % [nXn] relates states of prev step to curr step
0004         B % [nX1] relates optional control to state
0005         H % relates state to measurement
0006         R % measurement noise covariance
0007         Q % process noise covariance
0008     end % properties
0009     
0010     methods
0011         function kf = KalmanFilt()
0012         % PURPOSE: Constructor of Kalman Filter
0013         % INPUT
0014         %   none required
0015         % OUTPUT
0016         %   kf   - KalmanFilt object
0017                     
0018             r = .6;
0019             % Responsive Coeff. r->1: quickly believe in new measurement
0020             %                   r->0: stay consistent with the motion model
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 % constructor
0044         
0045         function track = predict(kf,track)
0046         % PURPOSE: To project state ahead
0047         % INPUT
0048         %   kf    - KalmanFilt object
0049         %   track - the WBCTrack object that being predicted
0050         % OUTPUT
0051         %   track - the WBCTrack object whose motion is predicted
0052         
0053             x = track.x;
0054             P = track.P;
0055 
0056             % x state matrix
0057             % x_ state priori
0058             % P_ error covariance priori
0059 %             if track.nCells==1
0060 %                 track.x_ = x;
0061 %                 track.P_ = kf.A*P*kf.A' + kf.Q;
0062 %             else
0063                if strcmp(track.Type,'LOST')==1 
0064                    %update on the direction of blood flow
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                %             end
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 % predict
0084         
0085         function track = update(kf,track)
0086             % x posterior state matrix
0087             % P posterior error covariance matrix
0088             % Priories
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                 % Position
0095                 cc = cell.Col;
0096                 cr = cell.Row;
0097                 pc = track.Cells(end-1).Col;
0098                 pr = track.Cells(end-1).Row;
0099                 % Velocity
0100                 dc = cc - pc;
0101                 dr = cr - pr;
0102 
0103 
0104                 % z measurement matrix
0105                 z = [cc cr dc dr]';
0106 
0107                 % Kalman Gain
0108                 K = P_*kf.H'/(kf.H*P_*kf.H' + kf.R);            
0109                 %if missing the measurement, keep the same estimate
0110                 if (~isempty(z))
0111                     % Update estimate with measurement
0112                     track.x = x_ + K*(z - kf.H*x_);
0113                 end  
0114                 
0115                 % Update error covariance
0116                 track.P = (eye(4) - K*kf.H)*P_;
0117                 % Update measurement
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                     % Add a Kalman cells
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                     % Now do update
0139                     x_ = track.x_;
0140                     P_ = track.P_;
0141                     cell = track.Cells(end);
0142                     % Position
0143                     cc = cell.Col;
0144                     cr = cell.Row;
0145                     pc = track.Cells(end-1).Col;
0146                     pr = track.Cells(end-1).Row;
0147                     % Velocity
0148                     dc = cc - pc;
0149                     dr = cr - pr;
0150                     % z measurement matrix
0151                     z = [cc cr dc dr]';
0152 
0153                     % Kalman Gain
0154                     K = P_*kf.H'/(kf.H*P_*kf.H' + kf.R);            
0155                     %if missing the measurement, keep the same estimate
0156                     if (~isempty(z))
0157                         % Update estimate with measurement
0158                         track.x = x_ + K*(z - kf.H*x_);
0159                     end  
0160 
0161                     % Update error covariance
0162                     track.P = (eye(4) - K*kf.H)*P_;
0163                     % Update measurement
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                     % remove all Kf prediction cells
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                 % update location but not velocity
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 % update
0187         
0188     end % methods
0189 end % KalmanFilt

Generated on Thu 17-Mar-2011 14:45:51 by m2html © 2005