Lines Matching refs:kalman
47 CvKalman *kalman = 0;
61 CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )));
62 memset( kalman, 0, sizeof(*kalman));
64 kalman->DP = DP;
65 kalman->MP = MP;
66 kalman->CP = CP;
68 CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ));
69 cvZero( kalman->state_pre );
71 CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ));
72 cvZero( kalman->state_post );
74 CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ));
75 cvSetIdentity( kalman->transition_matrix );
77 CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ));
78 cvSetIdentity( kalman->process_noise_cov );
80 CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ));
81 cvZero( kalman->measurement_matrix );
83 CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ));
84 cvSetIdentity( kalman->measurement_noise_cov );
86 CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ));
88 CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ));
89 cvZero( kalman->error_cov_post );
91 CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ));
95 CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ));
96 cvZero( kalman->control_matrix );
99 CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ));
100 CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ));
101 CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ));
102 CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ));
103 CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ));
106 kalman->PosterState = kalman->state_pre->data.fl;
107 kalman->PriorState = kalman->state_post->data.fl;
108 kalman->DynamMatr = kalman->transition_matrix->data.fl;
109 kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
110 kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
111 kalman->PNCovariance = kalman->process_noise_cov->data.fl;
112 kalman->KalmGainMatr = kalman->gain->data.fl;
113 kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
114 kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
120 cvReleaseKalman( &kalman );
122 return kalman;
129 CvKalman *kalman;
137 kalman = *_kalman;
140 cvReleaseMat( &kalman->state_pre );
141 cvReleaseMat( &kalman->state_post );
142 cvReleaseMat( &kalman->transition_matrix );
143 cvReleaseMat( &kalman->control_matrix );
144 cvReleaseMat( &kalman->measurement_matrix );
145 cvReleaseMat( &kalman->process_noise_cov );
146 cvReleaseMat( &kalman->measurement_noise_cov );
147 cvReleaseMat( &kalman->error_cov_pre );
148 cvReleaseMat( &kalman->gain );
149 cvReleaseMat( &kalman->error_cov_post );
150 cvReleaseMat( &kalman->temp1 );
151 cvReleaseMat( &kalman->temp2 );
152 cvReleaseMat( &kalman->temp3 );
153 cvReleaseMat( &kalman->temp4 );
154 cvReleaseMat( &kalman->temp5 );
156 memset( kalman, 0, sizeof(*kalman));
166 cvKalmanPredict( CvKalman* kalman, const CvMat* control )
174 if( !kalman )
179 CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ));
181 if( control && kalman->CP > 0 )
183 CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ));
187 CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ));
190 CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
191 kalman->error_cov_pre, CV_GEMM_B_T ));
193 result = kalman->state_pre;
202 cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
210 if( !kalman || !measurement )
214 CV_CALL( cvMatMulAdd( kalman->measurement_matrix,
215 kalman->error_cov_pre, 0, kalman->temp2 ));
217 CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
218 kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ));
221 CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ));
224 CV_CALL( cvTranspose( kalman->temp4, kalman->gain ));
227 CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ));
230 CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ));
233 CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
234 kalman->error_cov_post, 0 ));
236 result = kalman->state_post;
