|
| 1 | +#!/usr/bin/env python |
| 2 | +#-*- coding:utf-8 -*- |
| 3 | +# FileName: kalman2d.py |
| 4 | +# |
| 5 | +# Description: |
| 6 | +# |
| 7 | +# Version: 1.0 |
| 8 | +# Created: 2018-01-16 13:46:56 |
| 9 | +# Last Modified: 2018-01-17 14:12:46 |
| 10 | +# Revision: none |
| 11 | +# Compiler: gcc |
| 12 | +# |
| 13 | +# Author: zt () |
| 14 | +# Organization: |
| 15 | + |
| 16 | +''' |
| 17 | +kalman2d - 2D Kalman filter using OpenCV |
| 18 | +
|
| 19 | +Based on http://jayrambhia.wordpress.com/2012/07/26/kalman-filter/ |
| 20 | +
|
| 21 | +Copyright (C) 2014 Simon D. Levy |
| 22 | +
|
| 23 | +This code is free software: you can redistribute it and/or modify |
| 24 | +it under the terms of the GNU Lesser General Public License as |
| 25 | +published by the Free Software Foundation, either version 3 of the |
| 26 | +License, or (at your option) any later version. |
| 27 | +This code is distributed in the hope that it will be useful, |
| 28 | +
|
| 29 | +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 30 | +GNU General Public License for more details. |
| 31 | +
|
| 32 | +You should have received a copy of the GNU Lesser General Public License |
| 33 | +along with this code. If not, see <http://www.gnu.org/licenses/>. |
| 34 | +''' |
| 35 | + |
| 36 | +#from cv2 import cv |
| 37 | +import cv2 |
| 38 | +import numpy |
| 39 | + |
| 40 | +class Kalman2D(object): |
| 41 | + ''' |
| 42 | + A class for 2D Kalman filtering |
| 43 | + ''' |
| 44 | + |
| 45 | + def __init__(self, processNoiseCovariance=1e-4, measurementNoiseCovariance=1e-1, errorCovariancePost=0.1): |
| 46 | + ''' |
| 47 | + For explanation of the error covariances see |
| 48 | + http://en.wikipedia.org/wiki/Kalman_filter |
| 49 | + ''' |
| 50 | + # 状态空间:位置--2d,速度--2d |
| 51 | + self.kalman = cv2.KalmanFilter(4, 2, 0) |
| 52 | + #self.kalman_state = cv2.CreateMat(4, 1, cv.CV_32FC1) |
| 53 | + self.kalman_state = numpy.zeros((4,1),numpy.float32) |
| 54 | + self.kalman_process_noise = numpy.zeros((4,1),numpy.float32) |
| 55 | + #self.kalman_measurement = cv2.CreateMat(2, 1, cv.CV_32FC1) |
| 56 | + self.kalman_measurement = numpy.zeros((2,1),numpy.float32) |
| 57 | + |
| 58 | + for j in range(4): |
| 59 | + for k in range(4): |
| 60 | + self.kalman.transitionMatrix[j,k] = 0 |
| 61 | + self.kalman.transitionMatrix[j,j] = 1 |
| 62 | + # 加入速度 x = x + vx, y = y + vy |
| 63 | + # 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1 |
| 64 | + # 如果把下面两句注释掉,那么位置跟踪kalman滤波器的状态模型就是没有使用速度信息 |
| 65 | + # self.kalman.transition_matrix[0, 2]=1 |
| 66 | + # self.kalman.transition_matrix[1, 3]=1 |
| 67 | + |
| 68 | + cv2.setIdentity(self.kalman.measurementMatrix) |
| 69 | + #初始化带尺度的单位矩阵 |
| 70 | + cv2.setIdentity(self.kalman.processNoiseCov, cv2.RealScalar(processNoiseCovariance)) |
| 71 | + cv2.setIdentity(self.kalman.measurementNoiseCov, cv2.RealScalar(measurementNoiseCovariance)) |
| 72 | + cv2.setIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) |
| 73 | + |
| 74 | + self.predicted = None |
| 75 | + self.esitmated = None |
| 76 | + |
| 77 | + def update(self, x, y): |
| 78 | + ''' |
| 79 | + Updates the filter with a new X,Y measurement |
| 80 | + ''' |
| 81 | + self.kalman_measurement[0, 0] = x |
| 82 | + self.kalman_measurement[1, 0] = y |
| 83 | + |
| 84 | + self.predicted = cv2.KalmanFilter.predict(self.kalman) |
| 85 | + self.corrected = cv2.KalmanFilter.correct(self.kalman, self.kalman_measurement) |
| 86 | + |
| 87 | + def getEstimate(self): |
| 88 | + ''' |
| 89 | + Returns the current X,Y estimate. |
| 90 | + ''' |
| 91 | + return self.corrected[0,0], self.corrected[1,0] |
| 92 | + |
| 93 | + def getPrediction(self): |
| 94 | + ''' |
| 95 | + Returns the current X,Y prediction. |
| 96 | + ''' |
| 97 | + return self.predicted[0,0], self.predicted[1,0] |
| 98 | + |
0 commit comments