-
Notifications
You must be signed in to change notification settings - Fork 58
/
Copy pathstate_IMU_KF.py
124 lines (98 loc) · 3.72 KB
/
state_IMU_KF.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
# -*- coding: utf-8 -*-
"""
state_IMU_KF.py
author: Keita Nagara 永良慶太 (University of Tokyo) <nagara.keita()gmail.com>
State and estimation model of IMU with Kalman Filter.
This class is generated from "state.py".
"""
import sys
import math
import time
import copy
import cv2 as cv
import numpy as np
import KF
import Util
class StateIMUKF:
def __init__(self):
self.init()
def init(self):
self.isFirstTime = True
self.t = 0
self.t1 = 0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.initKalmanFilter()
def initKalmanFilter(self):
self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.sigma = np.zeros([12,12])
self.A = np.identity(12)
self.C = np.array([ [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
self.Q = np.diag([0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1]) # sys noise
self.R = np.diag([0.01,0.01,0.01,0.01,0.01,0.01]) # obs noise
"""
This method is called from "sensor.py" when new IMU sensor data are arrived.
time : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori, gyro):
self.t1 = self.t
self.t = time_
if(self.isFirstTime):
#init mu
self.mu = np.array([0.0,0.0,0.0,
0.0,0.0,0.0,
accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
else:
#start_time = time.clock()
#observation
Y = np.array([accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
dt = self.t - self.t1
dt2 = 0.5 * dt * dt
self.A = np.array([[1.0,0.0,0.0,dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0,0.0],
[0.0,1.0,0.0,0.0,dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0],
[0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,dt2,0.0,0.0,0.0],
[0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
"""
self.accel3 = copy.deepcopy(self.accel2)
self.accel2 = copy.deepcopy(self.accel1)
self.accel1 = copy.deepcopy(accel)
if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False):
self.mu[3] = 0.0
if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False):
self.mu[4] = 0.0
if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False):
self.mu[5] = 0.0
"""
self.mu, self.sigma = KF.execKF1Simple(Y,self.mu,self.sigma,self.A,self.C,self.Q,self.R)
#end_time = time.clock()
#print "%f" %(end_time-start_time)
if(self.isFirstTime):
self.isFirstTime = False
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = np.array([self.mu[0],self.mu[1],self.mu[2]])
v = np.array([self.mu[3],self.mu[4],self.mu[5]])
a = np.array([self.mu[6],self.mu[7],self.mu[8]])
o = np.array([self.mu[9],self.mu[10],self.mu[11]])
return x,v,a,o