CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place.

| Download
Project: test
Views: 91872
1
# -*- coding: utf-8 -*-
2
"""
3
Created on Mon Jul 13 13:20:27 2015
4
5
@author: Roger
6
"""
7
8
from filterpy.kalman import UnscentedKalmanFilter as UKF
9
from filterpy.kalman import JulierSigmaPoints
10
from math import atan2
11
import numpy as np
12
13
def hx(x):
14
""" compute measurements corresponding to state x"""
15
dx = x[0] - hx.radar_pos[0]
16
dy = x[1] - hx.radar_pos[1]
17
return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5])
18
19
def fx(x,dt):
20
""" predict state x at 'dt' time in the future"""
21
return x
22
23
def set_radar_pos(pos):
24
global hx
25
hx.radar_pos = pos
26
27
def sensor_fusion_kf():
28
global hx, fx
29
# create unscented Kalman filter with large initial uncertainty
30
points = JulierSigmaPoints(2, kappa=2)
31
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points)
32
kf.x = np.array([100, 100.])
33
kf.P *= 40
34
return kf
35
36
37