RinOS 16 Report post Posted September 16, 2009 Всем привет! В книжке по OpenCV есть пример по фильтру Калмана, несколько раз пытался понять листинг, но ни чего непонял... К тому же тут еще не понятно зачем введены шумы (x_k). Обясните пожалуйста этот листинг, можно вырезать все лишнее оставив, нужное, а именно (предсказывать где будет объект в следующий момент времени). #define phi2xy(mat) \ cvPoint( cvRound(img->width/2 + img->width/3*cos(mat->data.fl[0])),\ cvRound( img->height/2 - img->width/3*sin(mat->data.fl[0])) ) int main(int argc, char** argv) { // Initialize, create Kalman Filter object, window, random number // generator etc. // cvNamedWindow( "Kalman", 1 ); CvRandState rng; cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 ); CvKalman* kalman = cvCreateKalman( 2, 1, 0 ); // state is (phi, delta_phi) - angle and angular velocity // Initialize with random guess. // CvMat* x_k = cvCreateMat( 2, 1, CV_32FC1 ); cvRandSetRange( &rng, 0, 0.1, 0 ); rng.disttype = CV_RAND_NORMAL; cvRand( &rng, x_k ); // process noise // CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 ); // measurements, only one parameter for angle // CvMat* z_k = cvCreateMat( 1, 1, CV_32FC1 ); cvZero( z_k ); // Transition matrix 'F' describes relationship between // model parameters at step k and at step k+1 (this is // the "dynamics" in our model. // const float F[] = { 1, 1, 0, 1 }; memcpy( kalman->transition_matrix->data.fl, F, sizeof(F)); // Initialize other Kalman filter parameters. // cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) ); cvSetIdentity( kalman->error_cov_post, cvRealScalar(1)); // choose random initial state // cvRand( &rng, kalman->state_post ); while( 1 ) { // predict point position const CvMat* y_k = cvKalmanPredict( kalman, 0 ); // generate measurement (z_k) // cvRandSetRange( &rng, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0 ); cvRand( &rng, z_k ); cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k ); // plot points (eg convert to planar co-ordinates and draw) // cvZero( img ); cvCircle( img, phi2xy(z_k), 4, CVX_YELLOW ); // observed state cvCircle( img, phi2xy(y_k), 4, CVX_WHITE, 2 ); // "predicted" state cvCircle( img, phi2xy(x_k), 4, CVX_RED ); // real state cvShowImage( "Kalman", img ); // adjust Kalman filter state // cvKalmanCorrect( kalman, z_k ); // Apply the transition matrix 'F' (eg, step time forward) // and also apply the "process" noise w_k. // cvRandSetRange( &rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0 ); cvRand( &rng, w_k ); cvMatMulAdd( kalman->transition_matrix, x_k, w_k, x_k ); // exit if user hits 'Esc' if( cvWaitKey( 100 ) == 27 ) break; } Share this post Link to post Share on other sites
Smorodov 579 Report post Posted September 16, 2009 Листинг попытаюсь пояснить (что сам понял) потом. А пока вот что перевел (очень сырой вариант), может чего добавите Фильтр_Кальмана.rar Share this post Link to post Share on other sites
Smorodov 579 Report post Posted September 16, 2009 Листинг попытаюсь пояснить (что сам понял) потом. А пока вот что перевел (очень сырой вариант), может чего добавите Фильтр_Кальмана.rar Есть еще такой класс для двумерных координат, откопал на просторах и-нета (перевел часть комментов, но не тестил правда пока): Вот файл: Kalman2.rar, А вот что внутри: class KalmanFilter { public: KalmanFilter(); ~KalmanFilter(); virtual void predictionBegin(float x,float y); virtual void predictionUpdate(float x,float y); virtual void predictionReport(CvPoint &pnt); private: CvKalman *m_pKalmanFilter; CvRandState rng; CvMat* state; CvMat* process_noise; CvMat* measurement; CvMat* measurement_noise; bool m_bMeasurement; clock_t m_timeLastMeasurement; CRITICAL_SECTION mutexPrediction; }; // Инициализация фильтра Кальмана KalmanFilter::KalmanFilter() { m_timeLastMeasurement = clock(); int dynam_params = 4; // x,y,dx,dy int measure_params = 2; m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params); state = cvCreateMat( dynam_params, 1, CV_32FC1 ); // Генератор случайных чисел cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); cvRandSetRange( &rng, 0, 1, 0 ); rng.disttype = CV_RAND_NORMAL; cvRand( &rng, state ); process_noise = cvCreateMat( dynam_params, 1, CV_32FC1 ); // (w_k) measurement = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y (z_k) measurement_noise = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y (v_k) cvZero(measurement); // F matrix data // F is transition matrix. It relates how the states interact // For single input fixed velocity the new value // depends on the previous value and velocity- hence 1 0 1 0 // on top line. Новое значение скорости не зависит от предыдущего // значения координаты, и зависит только от предыдущей скорости- поэтому 0 1 0 1 on second row const float F[] = { 1, 0, 1, 0,//x + dx 0, 1, 0, 1,//y + dy 0, 0, 1, 0,//dx = dx 0, 0, 0, 1,//dy = dy }; memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F)); cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) ); // (H) cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-5) ); // (Q) cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); // (R) cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(1)); // choose random initial state cvRand( &rng, m_pKalmanFilter->state_post ); InitializeCriticalSection(&mutexPrediction); } KalmanFilter::~KalmanFilter() { // Деструктор cvReleaseMat( &state ); cvReleaseMat( &process_noise ); cvReleaseMat( &measurement ); cvReleaseMat( &measurement_noise ); cvReleaseKalman( &m_pKalmanFilter ); DeleteCriticalSection(&mutexPrediction); } // Устанавливаем значения переменных фильтра на первом шаге // Если этого не сделать, то объект не будет трекаться void KalmanFilter::predictionBegin(float x,float y) { //В переменной m_pKalmanFilter->state_post хранится предыдущее состояние m_pKalmanFilter->state_post->data.fl[0] = x; //координата x m_pKalmanFilter->state_post->data.fl[1] = y; //координата y m_pKalmanFilter->state_post->data.fl[2] = (float)0; //dx - скорость по x m_pKalmanFilter->state_post->data.fl[3] = (float)0; //dy - скорость по y } void KalmanFilter::predictionUpdate(float x,float y) { EnterCriticalSection(&mutexPrediction); state->data.fl[0] = x; //center x state->data.fl[1] = y; //center y m_bMeasurement = true; LeaveCriticalSection(&mutexPrediction); } void KalmanFilter::predictionReport(CvPoint &pnt) { clock_t timeCurrent = clock(); EnterCriticalSection(&mutexPrediction); const CvMat* prediction = cvKalmanPredict( m_pKalmanFilter, 0 ); pnt.x = prediction->data.fl[0]; pnt.y = prediction->data.fl[1]; // Если мы получили действительные данные недавно, тогда используем полученные данные для корректировки // состояния фильтра. Если мы не получили данные, тогда корректируем состояние фильтра, используя предсказанные значения if (m_bMeasurement) //(timeCurrent - m_timeLastMeasurement < TIME_OUT_LOCATION_UPDATE) // используем действительные данные { m_bMeasurement = false; } // Обычно фильтр работает быстрее чем поступают данные измерений.. // Таким образом, можно дать фильтру брать данные из предсказанных значений в промежутках между измерениями // (делает работу трекера более плавной) else // используем предсказанные данные { state->data.fl[0] = pnt.x; state->data.fl[1] = pnt.y; } // генерируем шум измерения noise(z_k) cvRandSetRange( &rng, 0, sqrt(m_pKalmanFilter->measurement_noise_cov->data.fl[0]), 0 ); cvRand( &rng, measurement_noise ); // zk = Hk * xk + vk // measurement = measurement_error_matrix * current_state + measurement_noise cvMatMulAdd( m_pKalmanFilter->measurement_matrix, state, measurement_noise, measurement ); // уточняем состояние фильтра cvKalmanCorrect( m_pKalmanFilter, measurement ); // гененрируем шум процесса(w_k) cvRandSetRange( &rng, 0, sqrt(m_pKalmanFilter->process_noise_cov->data.fl[0]), 0 ); cvRand( &rng, process_noise ); // xk = F * xk-1 + B * uk + wk // state = transition_matrix * previous_state + control_matrix * control_input + noise cvMatMulAdd( m_pKalmanFilter->transition_matrix, state, process_noise, state ); LeaveCriticalSection(&mutexPrediction); }[/code] Share this post Link to post Share on other sites
RinOS 16 Report post Posted September 21, 2009 Есть еще такой класс для двумерных координат, откопал на просторах и-нета (перевел часть комментов, но не тестил правда пока): За пример спасибо) С помощью него я понял что не могу использовать фильтр Кальмана в моём случае( Моя цель была найти объект в следующем кадре... Но что бы передать фильтру Кальмана predictionUpdate() надо знать где этот объект находиться... Share this post Link to post Share on other sites
Smorodov 579 Report post Posted November 30, 2009 Пример фильтра Кальмана + запись видео в файл. Используется OpenCV2.0. Его DLL надо покидать в видное программе место (можно к exe-шнику в папку) Архив с проектом: kalman_track.rar У меня путь к нему: C:\Program Files\Borland\CBuilder6\Projects\Cv2Programs\kalman_track\ Share this post Link to post Share on other sites
quosego 5 Report post Posted November 30, 2009 Помойму такие вещи можно складировать в вике, например в разделе туториалы. В дальнейшем будет проще искать и возможно дорабатывать. Share this post Link to post Share on other sites
Smorodov 579 Report post Posted November 30, 2009 Помойму такие вещи можно складировать в вике, например в разделе туториалы. В дальнейшем будет проще искать и возможно дорабатывать. Спасибо за совет. Я так и сделаю, пока не хочу там беспорядка. Форум просмотрю и вынесу такие примеры туда, к тому же еще текст надо к нему написать. Пока тут полежит - пройдет проверку, на нужность и заодно узнаю, какие вопросы вызывает программа. Share this post Link to post Share on other sites
gonzik 0 Report post Posted March 9, 2010 Пример фильтра Кальмана + запись видео в файл. Используется OpenCV2.0. Его DLL надо покидать в видное программе место (можно к exe-шнику в папку) Спасибо, пример очень порадовал! Как бы его переделать под VS2008... Share this post Link to post Share on other sites
Nuzhny 243 Report post Posted March 11, 2010 Как бы его переделать под VS2008...Если автор не против, то выложу переделку - делал как-то для себя, поиграть. Немного перегруппировал код, отформатировал по своему вкусу, но всё работает также как и в оригинале:#include "stdafx.h" #include <highgui.h> #include <cv.h> #pragma comment(lib, "cv.lib") #pragma comment(lib, "cxcore.lib") #pragma comment(lib, "highgui.lib") //////////////////////////////////////////////////////////////////////////// //function to find the biggest blob and to get the top left position CvPoint2D32f findBigBlob(IplImage *mask) { int maxval = 0; int minval = 100; //начинаем поиск контуров CvMemStorage *storage = cvCreateMemStorage(0); CvContourScanner traverse = cvStartFindContours(mask, storage, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE); //находим контуры с длиной периметра между minval и maxval CvSeq *mContour = 0; for (CvSeq *contour = cvFindNextContour(traverse); contour; contour = cvFindNextContour(traverse)) { int contourarea = (int)abs(cvContourArea(contour, CV_WHOLE_SEQ)); if (contourarea > maxval) { mContour = contour; maxval = contourarea; } } CvPoint2D32f ret_val = cvPoint2D32f(-1.0, -1.0); if (maxval > minval) { //находим центр масс CvMoments moments; cvMoments(mContour, &moments, 0); ret_val.x = (float)(moments.m10 / moments.m00); ret_val.y = (float)(moments.m01 / moments.m00); } //освобождаем память cvEndFindContours(&traverse); cvReleaseMemStorage(&storage); return ret_val; } //////////////////////////////////////////////////////////////////////////// //находим участки, подходящие нам по цвету (шайба) void region_color_range(const IplImage *color, IplImage *mask, uchar bmin, uchar bmax, uchar gmin, uchar gmax, uchar rmin, uchar rmax) { uchar *dst_ptr = (uchar *)mask->imageData; for (const uchar *src_ptr = (uchar *)color->imageData, *stop = src_ptr + color->imageSize; src_ptr != stop; src_ptr += color->nChannels) { uchar b = src_ptr[0]; uchar g = src_ptr[1]; uchar r = src_ptr[2]; //проверяем диапазон составляющих цвета if (b <= bmax && b >= bmin && g <= gmax && g >= gmin && r <= rmax && r >= rmin) *dst_ptr = 255; else *dst_ptr = 0; ++dst_ptr; } } //////////////////////////////////////////////////////////////////////////// int main() { //параметры, для идентификации шайбы по цвету uchar bmin = 0; uchar bmax = 51; uchar gmin = 120; uchar gmax = 225; uchar rmin = 102; uchar rmax = 235; CvMat *measurement = cvCreateMat(2, 1, CV_32FC1); cvZero(measurement); CvRNG rng = cvRNG(-1); CvMat *state = cvCreateMat(4, 1, CV_32FC1); // (x, y, deltax, deltay) cvRandArr(&rng, state, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1)); //стандартный набор функций для фильтра кальмана CvKalman *kalman = cvCreateKalman(4, 2, 0); //4 переменных состояния, 2 переменных измерения float deltatime = 50.f; //приращение времени float transition_matrix[] = { 1, 0, deltatime, 0, 0, 1, 0, deltatime, 0, 0, 1, 0, 0, 0, 0, 1}; //transition matrix memcpy(kalman->transition_matrix->data.fl, transition_matrix, sizeof(transition_matrix)); cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1)); cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5)); cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1)); cvSetIdentity(kalman->error_cov_post, cvRealScalar(1)); //выбираем случайное начальное состояние cvRandArr(&rng, kalman->state_post, CV_RAND_NORMAL, cvRealScalar(0), cvRealScalar(0.1)); cvNamedWindow("frame", 1); cvNamedWindow("color_segm", 1); cvNamedWindow("contours", 1); CvCapture *video = cvCaptureFromAVI("vid1.avi"); IplImage *mask = NULL; for (IplImage *frame = cvQueryFrame(video); frame; frame = cvQueryFrame(video)) { if (!mask) mask = cvCreateImage(cvSize(frame->width, frame->height), IPL_DEPTH_8U, 1); //находим положение шайбы, используя поиск по цвету region_color_range(frame, mask, bmin, bmax, gmin, gmax, rmin, rmax); cvShowImage("color_segm", mask); CvPoint2D32f position = findBigBlob(mask); cvShowImage("contours", mask); const CvMat *kalman_predict = cvKalmanPredict(kalman, 0); float predict_x = kalman_predict->data.fl[0]; float predict_y = kalman_predict->data.fl[1]; if (position.x < 0.0) { //уточняем используя предсказание measurement->data.fl[0] = predict_x; measurement->data.fl[1] = predict_y; } else { //уточняем, используя данные измерений measurement->data.fl[0] = position.x; measurement->data.fl[1] = position.y; } //Предсказанное и измеренное положение центра объекта cvDrawCircle(frame, cvPoint((int)position.x, (int)position.y), 3, cvScalar(255, 255, 255), -1, 8, 0); cvDrawCircle(frame, cvPoint((int)predict_x, (int)predict_y), 3, cvScalar(0, 0, 0), -1, 8, 0); //обновляем состояние фильтра cvKalmanCorrect(kalman, measurement); cvWaitKey(100); cvShowImage("frame", frame); } cvReleaseCapture(&video); if (mask) cvReleaseImage(&mask); cvDestroyWindow("frame"); cvDestroyWindow("color_segm"); cvDestroyWindow("contours"); return 0; } ////////////////////////////////////////////////////////////////////////////[/codebox] Share this post Link to post Share on other sites
gonzik 0 Report post Posted March 14, 2010 Если автор не против, то выложу переделку - делал как-то для себя, поиграть. Немного перегруппировал код, отформатировал по своему вкусу, но всё работает также как и в оригинале: Спасибо большое за помощь! Буду теперь пытаться решить поставленную задачу: Вместо шарика у меня должно быть лицо человека, и прямоугольник, захватывающий его, прямоугольник мне и надо сопровождать, при перемещении лица. В идеале хотелось бы вообще нужное лицо из толпы выделять мышкой и дальше его сопровождать... Буду пробовать Share this post Link to post Share on other sites
RinOS 16 Report post Posted July 14, 2011 Подниму темку) Как использовать фильтр Калмана для 3D трекинга? Кому ни будь попадалась на руки реализация по мимо этой: http://www.roninworks.com/?p=218 Share this post Link to post Share on other sites
Smorodov 579 Report post Posted July 14, 2011 Презентация с хорошим описанием есть здесь: http://www.probabilistic-robotics.org/ (книжка, кстати тоже интересная ) Из объяснения понятно, что для корректного использования фильтра нужна динамическая модель системы, которая затем преобразуется в матрицу переходов.Матрица переходов содержит вероятности переходов из состояния A в состояние Б. См. Марковские процессы. Для фильтрации координат, обычно, в качестве модели используют линеаризованные дифференциальные уравнения движения. В случае линейных перемещений они тривиальны X'=X'+X"*dt; X=X+X'*dt;. В случае добавления вращения лучше перейти к плюккеровым (Plukker) координатам и винтовому исчислению. Вот тут уже посложнее будет и матрицы будут 6х6. Именно вследствие сильной зависимости от параметров модели (механических, статистических, и т.д.), как мне кажется, универсальные реализации и не получили широкого распространения. Share this post Link to post Share on other sites
RinOS 16 Report post Posted July 14, 2011 >В случае добавления вращения Наверно глубины? (третье измерение) Буквально прям сейчас нашел реализацию, вроде 3D но матрица создается 6х3, оно ли это))? #ifndef _KALMAN_FILTER_H_ #define _KALMAN_FILTER_H_ #include <vector> #include <cv.h> #include "base.h" #include "Structures/Vector.h" #include "Structures/Calibration.h" namespace windage { namespace Algorithms { /** * @defgroup Algorithms Algorithm classes * @brief * algorithm classes * @addtogroup Algorithms * @{ */ /** * @defgroup AlgorithmsPoseEstimator Pose Estimator * @brief camera pose estimator in 3D * @addtogroup AlgorithmsPoseEstimator * @{ */ /** * @brief class for kalman filtering to prevent zitering * @author Woonhyuk Baek */ class DLLEXPORT KalmanFilter { private: /** Tx, Ty, Tz, dTx, dTy, dTz */ CvKalman* kalman; CvMat* measurement; public: virtual char* GetFunctionName(){return "KalmanFilter";}; KalmanFilter() { kalman = cvCreateKalman( 6, 3, 0 ); measurement = cvCreateMat( 3, 1, CV_32FC1 ); this->Initialize(); } ~KalmanFilter() { if(kalman) cvReleaseKalman(&kalman); if(measurement) cvReleaseMat(&measurement); } void Initialize(); windage::Vector3 Predict(); bool Correct(windage::Vector3 T); }; /** @} */ // addtogroup AlgorithmsPoseEstimator /** @} */ // addtogroup Algorithms } } #endif // _KALMAN_FILTER_H_ #include "Algorithms/KalmanFilter.h" using namespace windage; using namespace windage::Algorithms; void KalmanFilter::Initialize() { const float A[] = { 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,}; memcpy( this->kalman->transition_matrix->data.fl, A, sizeof(A)); cvSetIdentity( this->kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( this->kalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( this->kalman->measurement_noise_cov, cvRealScalar(1e-1) ); cvSetIdentity( this->kalman->error_cov_post, cvRealScalar(1)); } windage::Vector3 KalmanFilter::Predict() { windage::Vector3 prediction; const CvMat* predictionMat = cvKalmanPredict( this->kalman, 0 ); prediction.x = (double)predictionMat->data.fl[0]; prediction.y = (double)predictionMat->data.fl[1]; prediction.z = (double)predictionMat->data.fl[2]; return prediction; } bool KalmanFilter::Correct(windage::Vector3 T) { measurement->data.fl[0] = (float)T.x; measurement->data.fl[1] = (float)T.y; measurement->data.fl[2] = (float)T.z; cvKalmanCorrect( kalman, measurement ); return true; } 1 Share this post Link to post Share on other sites
Smorodov 579 Report post Posted July 14, 2011 Извиняюсь, напутал матрицы с учетом вращения будут матрица А например 12х12 (если не учитывать ускорения). Три линейных координаты это как раз тот тривиальный случай. Если Напишите уравнения по этим матрицам, то и получите для каждой координаты то что я написал выше. Только без учета ускорения: Вот строка из матрицы А: 1, 0, 0, 1, 0, 0 означает x=1*x+0*y+0*z+1*vx+0*vy+0*vz => x=x+vx*dt здесь (dt = 1), аналогично для y и z. Все скорости равны сами себе vx=vx; vy=vy; vz=vz; Так же видно что все координаты независимы, что не верно в случае учета поворота. Share this post Link to post Share on other sites
RinOS 16 Report post Posted July 18, 2011 Потестил этот код, неплохо ведет цель. X Y Z predict -0.466858 0.808474 2.26073 real -0.309018 0.486225 1.53683 -0.423659 0.684607 2.0921 -0.296634 0.412955 1.49816 -0.358051 0.502584 1.80924 -0.284429 0.322137 1.45931 -0.302884 0.319587 1.56944 -0.24458 0.255752 1.44207 -0.23795 0.192976 1.43535 -0.23719 0.200144 1.42542 -0.211506 0.116637 1.37693 -0.21606 0.155456 1.42932 -0.190204 0.0749303 1.37846 -0.202409 0.0838827 1.41801 -0.178239 0.0161604 1.38489 -0.193323 0.047973 1.42875 -0.173382 -0.0163037 1.40925 -0.185071 -0.00390054 1.43147 -0.170241 -0.0576007 1.42664 -0.18052 -0.0539902 1.43713 -0.169671 -0.103587 1.43971 -0.185037 -0.0819764 1.44412 -0.177794 -0.130038 1.4502 -0.20676 -0.128047 1.44989 -0.205006 -0.169031 1.45739 -0.239146 -0.156521 1.45426 -0.247401 -0.196648 1.46161 -0.260618 -0.190315 1.46437 -0.280221 -0.226259 1.47075 -0.27879 -0.215011 1.47307 Хочу реализовать мультитрекинг, есть у кого ни будь примеры или инфа? Свой уже написал но довольно косячно работает путает цели которые пересекаются. Share this post Link to post Share on other sites
Smorodov 579 Report post Posted July 18, 2011 В мультитрекинге, при пересечении траекторий, объекты можно по направлению вектора скорости различать. Т.к. скорость обычно мгновенно не меняется, особенно у массивных объектов. Share this post Link to post Share on other sites
Smorodov 579 Report post Posted July 22, 2011 Определение ориентации (в кватернионном представлении) на основе показаний акселерометра и магнетометра: http://code.google.com/p/9dof-orientation-estimation/ Хороший теоретический материал на англ. http://campar.in.tum.de/Chair/KalmanFilter Share this post Link to post Share on other sites
tipabot 0 Report post Posted May 14, 2012 Случайно удалил, сообщение, извиняюсь. (Smorodov) Share this post Link to post Share on other sites
Smorodov 579 Report post Posted May 14, 2012 При задании матрицы перехода используются школьные формулы (раздел механики, движение материальной точки) X = 1*X + 0*Y + Vx*deltatime + Vy*0, Y = 0*X + 1*Y + 0*Vx + Vy*deltatime, Vx = 0*X + 0*Y + 1*Vx + 0*Vy, Vy = 0*X + 0*Y + 0*Vx + 1*Vy[/code] deltatime - малый интервал времени, который задает скорость реакции фильтра (при уменьшении deltatime можно это видеть как эффект увеличение инерции, хотя это несколько другое) В общем если полностью, то так: Для оси X: ax=M/Fx; vx=vx+ax*dt; x=x+vx*dt где M - масса тела, ax - ускорение вдоль x, vx - скорость вдоль x, x - координата, dt - малый интервал времени, Fx - приложенное к точке усилие вдоль оси x. Share this post Link to post Share on other sites
tipabot 0 Report post Posted May 17, 2012 а где можно прочитать (желательно на руском так как математический английский очень слаб) или кто пояснит, про другие матрицы и почему мы используем именно эти значения (часть я понимаю но целой картины нет) Именно то что описанно в примере cvSetIdentity( this->kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( this->kalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( this->kalman->measurement_noise_cov, cvRealScalar(1e-1) ); cvSetIdentity( this->kalman->error_cov_post, cvRealScalar(1)); Читал то что описаннов opencv-книге помогло отчасти, но повторюсь нет общей картины Share this post Link to post Share on other sites
Smorodov 579 Report post Posted May 17, 2012 а где можно прочитать (желательно на руском так как математический английский очень слаб) или кто пояснит, про другие матрицы и почему мы используем именно эти значения (часть я понимаю но целой картины нет) Именно то что описанно в примере cvSetIdentity( this->kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( this->kalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( this->kalman->measurement_noise_cov, cvRealScalar(1e-1) ); cvSetIdentity( this->kalman->error_cov_post, cvRealScalar(1)); [/code] Читал то что описаннов opencv-книге помогло отчасти, но повторюсь нет общей картины Вот кусок моей незаконченной статьи: KalmanFilter.pdf Share this post Link to post Share on other sites
HITech 1 Report post Posted September 22, 2012 кто пояснит, про другие матрицы и почему мы используем именно эти значения (часть я понимаю но целой картины нет) Именно то что описанно в примере cvSetIdentity( this->kalman->measurement_matrix, cvRealScalar(1) ); cvSetIdentity( this->kalman->process_noise_cov, cvRealScalar(1e-5) ); cvSetIdentity( this->kalman->measurement_noise_cov, cvRealScalar(1e-1) ); cvSetIdentity( this->kalman->error_cov_post, cvRealScalar(1)); Это матрицы измерения, ковариации шума процесса, ковариации шума измерения, начальная матрица ковариации ошибки соответственно. Про первый листинг в теме пока не могу ничего сказать, а вот для второго листинга и далее такое определение матрицы ковариации шума процесса и переходной матрицы не совсем корректно. Если только шаг решения задачи у вас не 1 секунда. В противном случае эти матрицы будут иметь другой вид. Share this post Link to post Share on other sites
Smorodov 579 Report post Posted September 22, 2012 Ну почему обязательно 1 секунда? 1 мс, 1 мкс, 1час, 1 такт, 1 цикл, здесь ничего не сказано о единицах измерения, важно чтобы эти единицы сильно не менялись во время работы программы и у всех величин одной природы были одинаковые единицы измерения. И не проводить операции над величинами, обозначающими одну и ту же физическую величину, представленными в разных единицах измерения ([м]+[пикселы]) или ([с]+[мс]). Аналогичная ситуация с координатами (1 м, 1 пиксел,...). Share this post Link to post Share on other sites
HITech 1 Report post Posted September 29, 2012 Ну почему обязательно 1 секунда? 1 мс, 1 мкс, 1час, 1 такт, 1 цикл, здесь ничего не сказано о единицах измерения, важно чтобы эти единицы сильно не менялись во время работы программы и у всех величин одной природы были одинаковые единицы измерения. И не проводить операции над величинами, обозначающими одну и ту же физическую величину, представленными в разных единицах измерения ([м]+[пикселы]) или ([с]+[мс]). Аналогичная ситуация с координатами (1 м, 1 пиксел,...). Ну тогда и скорость у вас должна измеряться в соответствующих единицах: 1 метр/цикл, 1 пиксел/такт А если для примера взять систему уравнений, в которой вектор состояния это расстояние и скорость, то в дискретном представлении имеем следующие матрицы (если их расчитывать по теории): вектор состояния: x=[r v] матрица перехода: Fk=[1 dt; 0 1] матрица ковариации шума процесса: Qk=[(dt^3)/3 (dt^2)/2; (dt^2)/2 dt]Q где dt-шаг по времени. Матрицы представлены в таком же виде, как это делают в матлабе, но суть думаю будет ясна. А тут уж решайте сами, что это - мкс , цикл или такт Share this post Link to post Share on other sites
Smorodov 579 Report post Posted September 29, 2012 Ну у Вас опечатка наверное, в wiki http://en.wikipedia.org/wiki/Kalman_filter Это немного по-другому: Share this post Link to post Share on other sites
