You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
37 lines
950 B
37 lines
950 B
6 years ago
|
# cython: language_level=3
|
||
|
|
|
||
|
|
cdef class KF1D:
|
||
|
|
def __init__(self, x0, A, C, K):
|
||
|
|
self.x0_0 = x0[0][0]
|
||
|
|
self.x1_0 = x0[1][0]
|
||
|
|
self.A0_0 = A[0][0]
|
||
|
|
self.A0_1 = A[0][1]
|
||
|
|
self.A1_0 = A[1][0]
|
||
|
|
self.A1_1 = A[1][1]
|
||
|
|
self.C0_0 = C[0]
|
||
|
|
self.C0_1 = C[1]
|
||
|
|
self.K0_0 = K[0][0]
|
||
|
|
self.K1_0 = K[1][0]
|
||
|
|
|
||
|
|
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
|
||
|
|
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
|
||
|
|
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
|
||
|
|
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
|
||
|
|
|
||
|
|
def update(self, meas):
|
||
|
|
cdef double x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
|
||
|
|
cdef double x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
|
||
|
|
self.x0_0 = x0_0
|
||
|
|
self.x1_0 = x1_0
|
||
|
|
|
||
|
|
return [self.x0_0, self.x1_0]
|
||
|
|
|
||
|
|
@property
|
||
|
|
def x(self):
|
||
|
|
return [[self.x0_0], [self.x1_0]]
|
||
|
|
|
||
|
|
@x.setter
|
||
|
|
def x(self, x):
|
||
|
|
self.x0_0 = x[0][0]
|
||
|
|
self.x1_0 = x[1][0]
|