칼 만 필터 python 약 해

목표 추적 을 하고 싶 기 때문에 가장 전형 적 인 칼 만 필터 와 헝가리 알고리즘 으로 예측 할 수 있다.
칼 만 필터
유명한 칼 만 필 터 는 실제 적 으로 지난 시간 목표 의 위치 와 이 시간 에 센서 나 다른 방법 으로 측정 한 목표 위 치 를 이용 하여 실제 목 표를 평가 하 는 알고리즘 이다.
현재 시각 에 측정 한 목표 위 치 는 오차 나 소음 이 존재 하기 때문에 직접 사용 할 수 없 으 며, 예측 량 은 당사자 가 신 봉 이 아니 기 때문에 예언 할 수 없 기 때문에 직접 사용 할 수 없다. 그러면 어떻게 해 야 이 두 가지 편차 가 있 는 결 과 를 가장 효과적으로 이용 하여 실제 에 가장 가 까 운 결 과 를 얻 을 수 있 습 니까?그래서 서로 다른 필터 링 알고리즘 이 생 겨 나 는데...
수학 원리
전제: 모든 위치 변 화 는 선형 이다.
칼 만 필 터 는 신호 와 소음 이 모두 안정 적 인 과정 이라는 가설 조건 을 요구 하지 않 는 다.매 시간의 시스템 스 포일 러 와 관측 오차 (즉 소음) 에 대해 그들의 통계 적 성질 에 대해 적당 한 가정 을 하고 소음 이 함 유 된 관측 신 호 를 처리 하면 평균 적 인 의미 에서 오 차 를 최소 의 진실 신호 로 평가 할 수 있다.
형식 설명
X(k)=A X(k-1)+B U(k)+W(k) Z(k)=H X(k)+V(k)
상기 등식 은 분 산 된 시스템 의 상태 공간 모델 X (k), X (k - 1) 는 k 와 k - 1 시간 시스템 의 상태 변수 A 를 대표 하고 B 는 시스템 매개 변수 U (k) 는 k 시간 에 시스템 에 대한 통 제 량 Z (k) 는 k 시간의 측정 값 H 는 측정 시스템 의 매개 변수 W (k) 와 V (k) 는 각각 과정 과 측정 의 소음 을 나타 낸다.
예 를 들 어 저격 수 는 목 표를 저격 하고 조준 경 에 있 는 것 은 목표 의 현재 위치 이 며 총 을 쏘 는 것 은 반드시 목표 의 다음 시간의 위치 이 어야 하기 때문에 첫 번 째 등식 은 내 가 지난 시간 에 조준 경 에서 목 표를 보 았 고 이 시각 에 그 위 치 를 예측 한 것 과 같다.두 번 째 등식 은 나의 눈 근시 에 해당 하고 현재 시각 에 보 이 는 위치 도 정확 하지 않 으 며 소음 이 존재 한다.
상기 사례 를 통 해 알 수 있 듯 이 만약 에 선형 사상 을 사용 하면 우 리 는 지난 순간 에 우리 가 현재 시각 에 측정 치 를 예측 한 다음 에 이들 을 상쇄 하면 우리 가 예측 한 오 차 를 얻 을 수 있다. 그 다음 에 이 오 차 를 일정한 비례 에 따라 우리 가 실제 위치 에 대한 예측 에 보충 하면 가능 한 한 진실 치 에 가 까 워 질 수 있다. 비록 약간 복잡 하지만근 데 생각해 보면 납득 이 가.그래서 생 겼 다.
X’(k)=A X*(k-1)+B U(k)+W(k) Z’(k)=H X’(k)+V(k)
X * (k) = X '(k) + Kn (Z (k) - Z' (k) X * 는 추정 치, X ', Z' 는 예측 치, Z 는 측정 치, Kn 은 우리 가 선택 한 계수 다.
더 나 아가 X * (k) = X '(k) + Kn (Z (k) - (H X' (k) + V (k)) 를 얻는다.
칼 만 필 터 는 협 방 차 행렬 로 Kn 를 구한다.
추정 값 과 실제 값 의 협 방 차 행렬 P (k) = E [(X (k) - X * (k)] x [(X (k) - X * (k)] ^ T] 예측 값 과 실제 값 의 협 방 차 행렬 P '(k) = E [(X (k) - X' (k)] x [(X (k) - X '(k)] ^ T] 이전 예측 값 과 추정 값 의 관 계 를 구 해 내 면 Kn = P' (k) / (k) + R)그 중에서 R 은 소음 의 협 방 차 행렬 로 서열 을 쉽게 미 루 기 위해 P '(k) 의 전달 형식: P' (k + 1) = AP '(k) A ^ T + Q 를 도입 합 니 다.
그래서 칼 만 필 터 는 주로 예측 과 업데이트 두 단계 로 나 뉘 어 교체 진행 된다.
다음은 OpenCV 와 봉 인 된 cv2. kalman Filter 를 보고 마우스 궤적 추적 을 실현 합 니 다.
가방 가 져 오기, 800 * 800 빈 프레임 만 들 기, 측정 값 초기 화 및 예측 값

import cv2
import numpy as np

frame = np.zeros((800, 800, 3), np.uint8)
last_measurement = current_measurement = np.array((2,1), np.float32)
last_prediction = current_prediction = np.zeros((2,1), np.float320)

마우스 이동 반전 함 수 를 정의 하여 추적 효 과 를 그립 니 다.

def mousemove(event, x, y, s, p):
    global frame, current_measurement, measurements, last_measurement,current_prediction,last_prediction
    #     
    last_prediction = current_prediction
    last_measurement = current_measurement
    current_measurement = np.array([[np.float32(x)],[np.float32(y)]])
    #                
    kalman.correct(current_measurement)
    current_prediction = kalman.predict()

    lmx, lmy = last_measurement[0], last_measurement[1]
    cmx, cmy = current_measurement[0], current_measurement[1]
    lpx, lpy = last_prediction[0], last_prediction[1]
    cpx, cpy = current_prediction[0], current_prediction[1]
    cv2.line(frame, (lmx, lmy), (cmx, cmy), (255, 0, 0))
    cv2.line(frame, (lpx, lpy), (cpx, cpy), (255, 0, 255))

창 보이 기

cv2.namedWindow("kalman_tracker")
cv2.setMouseCallback("kalman_tracker", mousemove)

kalman = cv2.KalmanFilter(4,2)
kalman.measurementMatrix = np.array([[1,0,0,0], [0,1,0,0]], np.float32)
kalman.transitionMatrix = np.array([[1,0,1,0], [0,1,0,1], [0,0,1,0], [0,0,0,1]], np.float32)
kalman.processNoiseCov = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], np.float32)*0.0

while True:
    cv2.imshow("kalman_tracker", frame)
    if cv2.waitKey(30) & 0xFF == 27:
        break

cv2.destroyAllWindows()