Переполнение стека в OpenCV Kalman Filter

Я пытаюсь реализовать фильтр Калмана для 3D-отслеживания в OpenCV 2.2. Переменными состояния являются координаты x, y, z, за которыми следуют скорости Vx, Vy и Vz, и я могу измерять только x, y и z.

Для начала я использовал пример из книги «Изучение OpenCV от O’reilly», но когда я попытался адаптировать этот пример к моей проблеме, вещи немного запутались.

Это моя реализация (я пытался свести код только к соответствующим частям, и я много прокомментировал, чтобы облегчить чтение).

    CvKalman* kalman = cvCreateKalman( 6, 3, 0 );

// Setting the initial state estimates to [0,0,0,0,0,0].
CvMat* x_k = cvCreateMat( 6, 1, CV_32FC1 );
cvZero(x_k);

// Setting the a posteriori estimate to zero.
cvZero(kalman->state_post);

// Creating the process noise vector.
CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );

// Creating the measurement vector.
CvMat* z_k = cvCreateMat( 6, 1, CV_32FC1 );
cvZero( z_k );

// Initializing the state transition matrix.
float F_kalman[] = { 1,0,0,0.05,0,0, 0,1,0,0,0.05,0, 0,0,1,0,0,0.05, 0,0,0,1,0,0, 0,0,0,0,0,1 };
memcpy( kalman->transition_matrix->data.fl, F_kalman, sizeof(F_kalman));

// Initializing the other necessary parameters for the filter.
cvSetIdentity( kalman->measurement_matrix);
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-2) );
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));

// Updates the measurement vector with my sensor values, wich were in the variable xyz (an array of CvScalar).
cvSetReal1D(z_k,0,xyz[i].val[0]);
cvSetReal1D(z_k,1,xyz[i].val[1]);
cvSetReal1D(z_k,2,xyz[i].val[2]);

cvKalmanPredict(kalman,0);
cvKalmanCorrect(kalman,z_k);

Проблема в том, что когда я запускаю код, я получаю «Необработанное исключение 0x55a3e757 в test.exe: 0xC00000FD: переполнение стека». в строке cvKalmanCorrect.

Возможно, я инициализировал одну из матриц неправильным ожидаемым размером, но я действительно потерял, как это проверить.

Какие-нибудь мысли?