# -*- coding: utf-8 -*- """ Created on Sun May 24 08:39:36 2015 @author: Roger """ #x = x x' y y' theta from math import cos, sin, sqrt, atan2 import numpy as np from numpy import array, dot from numpy.linalg import pinv def print_x(x): print(x[0, 0], x[1, 0], np.degrees(x[2, 0])) def control_update(x, u, dt): """ x is [x, y, hdg], u is [vel, omega] """ v = u[0] w = u[1] if w == 0: # approximate straight line with huge radius w = 1.e-30 r = v/w # radius return x + np.array([[-r*sin(x[2]) + r*sin(x[2] + w*dt)], [ r*cos(x[2]) - r*cos(x[2] + w*dt)], [w*dt]]) a1 = 0.001 a2 = 0.001 a3 = 0.001 a4 = 0.001 sigma_r = 0.1 sigma_h = a_error = np.radians(1) sigma_s = 0.00001 def normalize_angle(x, index): if x[index] > np.pi: x[index] -= 2*np.pi if x[index] < -np.pi: x[index] = 2*np.pi def ekfloc_predict(x, P, u, dt): h = x[2] v = u[0] w = u[1] if w == 0: # approximate straight line with huge radius w = 1.e-30 r = v/w # radius sinh = sin(h) sinhwdt = sin(h + w*dt) cosh = cos(h) coshwdt = cos(h + w*dt) G = array( [[1, 0, -r*cosh + r*coshwdt], [0, 1, -r*sinh + r*sinhwdt], [0, 0, 1]]) V = array( [[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w], [(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w], [0, dt]]) # covariance of motion noise in control space M = array([[a1*v**2 + a2*w**2, 0], [0, a3*v**2 + a4*w**2]]) x = x + array([[-r*sinh + r*sinhwdt], [r*cosh - r*coshwdt], [w*dt]]) P = dot(G, P).dot(G.T) + dot(V, M).dot(V.T) return x, P def ekfloc(x, P, u, zs, c, m, dt): h = x[2] v = u[0] w = u[1] if w == 0: # approximate straight line with huge radius w = 1.e-30 r = v/w # radius sinh = sin(h) sinhwdt = sin(h + w*dt) cosh = cos(h) coshwdt = cos(h + w*dt) F = array( [[1, 0, -r*cosh + r*coshwdt], [0, 1, -r*sinh + r*sinhwdt], [0, 0, 1]]) V = array( [[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w], [(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w], [0, dt]]) # covariance of motion noise in control space M = array([[a1*v**2 + a2*w**2, 0], [0, a3*v**2 + a4*w**2]]) x = x + array([[-r*sinh + r*sinhwdt], [r*cosh - r*coshwdt], [w*dt]]) P = dot(F, P).dot(F.T) + dot(V, M).dot(V.T) R = np.diag([sigma_r**2, sigma_h**2, sigma_s**2]) for i, z in enumerate(zs): j = c[i] q = (m[j][0] - x[0, 0])**2 + (m[j][1] - x[1, 0])**2 z_est = array([sqrt(q), atan2(m[j][1] - x[1, 0], m[j][0] - x[0, 0]) - x[2, 0], 0]) H = array( [[-(m[j, 0] - x[0, 0]) / sqrt(q), -(m[j, 1] - x[1, 0]) / sqrt(q), 0], [ (m[j, 1] - x[1, 0]) / q, -(m[j, 0] - x[0, 0]) / q, -1], [0, 0, 0]]) S = dot(H, P).dot(H.T) + R #print('S', S) K = dot(P, H.T).dot(pinv(S)) y = z - z_est normalize_angle(y, 1) y = array([y]).T #print('y', y) x = x + dot(K, y) I = np.eye(P.shape[0]) I_KH = I - dot(K, H) #print('i', I_KH) P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T) return x, P def ekfloc2(x, P, u, zs, c, m, dt): h = x[2] v = u[0] w = u[1] if w == 0: # approximate straight line with huge radius w = 1.e-30 r = v/w # radius sinh = sin(h) sinhwdt = sin(h + w*dt) cosh = cos(h) coshwdt = cos(h + w*dt) F = array( [[1, 0, -r*cosh + r*coshwdt], [0, 1, -r*sinh + r*sinhwdt], [0, 0, 1]]) V = array( [[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w], [(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w], [0, dt]]) # covariance of motion noise in control space M = array([[a1*v**2 + a2*w**2, 0], [0, a3*v**2 + a4*w**2]]) x = x + array([[-r*sinh + r*sinhwdt], [r*cosh - r*coshwdt], [w*dt]]) P = dot(F, P).dot(F.T) + dot(V, M).dot(V.T) R = np.diag([sigma_r**2, sigma_h**2]) for i, z in enumerate(zs): j = c[i] q = (m[j][0] - x[0, 0])**2 + (m[j][1] - x[1, 0])**2 z_est = array([sqrt(q), atan2(m[j][1] - x[1, 0], m[j][0] - x[0, 0]) - x[2, 0]]) H = array( [[-(m[j, 0] - x[0, 0]) / sqrt(q), -(m[j, 1] - x[1, 0]) / sqrt(q), 0], [ (m[j, 1] - x[1, 0]) / q, -(m[j, 0] - x[0, 0]) / q, -1]]) S = dot(H, P).dot(H.T) + R #print('S', S) K = dot(P, H.T).dot(pinv(S)) y = z - z_est normalize_angle(y, 1) y = array([y]).T #print('y', y) x = x + dot(K, y) print('x', x) I = np.eye(P.shape[0]) I_KH = I - dot(K, H) P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T) return x, P m = array([[5, 5], [7,6], [4, 8]]) x = array([[2, 6, .3]]).T u = array([.5, .01]) P = np.diag([1., 1., 1.]) c = [0, 1, 2] import matplotlib.pyplot as plt from numpy.random import randn from filterpy.common import plot_covariance_ellipse from filterpy.kalman import KalmanFilter plt.figure() plt.plot(m[:, 0], m[:, 1], 'o') plt.plot(x[0], x[1], 'x', color='b', ms=20) xp = x.copy() dt = 0.1 np.random.seed(1234) for i in range(1000): xp, _ = ekfloc_predict(xp, P, u, dt) plt.plot(xp[0], xp[1], 'x', color='g', ms=20) if i % 10 == 0: zs = [] for lmark in m: d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h zs.append(np.array([d, a])) x, P = ekfloc2(x, P, u, zs, c, m, dt*10) if P[0,0] < 10000: plot_covariance_ellipse((x[0,0], x[1,0]), P[0:2, 0:2], std=2, facecolor='g', alpha=0.3) plt.plot(x[0], x[1], 'x', color='r') plt.axis('equal') plt.show()