Skip to content

Commit f3f4619

Browse files
some changes
1 parent 30d15bf commit f3f4619

1 file changed

Lines changed: 12 additions & 11 deletions

File tree

src/smsfusion/_smoothing.py

Lines changed: 12 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ def position(self) -> NDArray:
150150
"""
151151
x = self.x
152152
if x.size == 0:
153-
return np.array([])
153+
return np.empty((0, 3))
154154
return self.x[:, :3]
155155

156156
def velocity(self) -> NDArray:
@@ -165,7 +165,7 @@ def velocity(self) -> NDArray:
165165
"""
166166
x = self.x
167167
if x.size == 0:
168-
return np.array([])
168+
return np.empty((0, 3))
169169
return self.x[:, 3:6]
170170

171171
def quaternion(self) -> NDArray:
@@ -180,7 +180,7 @@ def quaternion(self) -> NDArray:
180180
"""
181181
x = self.x
182182
if x.size == 0:
183-
return np.array([])
183+
return np.empty((0, 4))
184184
return self.x[:, 6:10]
185185

186186
def bias_acc(self) -> NDArray:
@@ -195,8 +195,8 @@ def bias_acc(self) -> NDArray:
195195
"""
196196
x = self.x
197197
if x.size == 0:
198-
return np.array([])
199-
return self.x[:, 10:13]
198+
return np.empty((0, 3))
199+
return x[:, 10:13]
200200

201201
def bias_gyro(self, degrees: bool = False) -> NDArray:
202202
"""
@@ -208,7 +208,8 @@ def bias_gyro(self, degrees: bool = False) -> NDArray:
208208
Gyroscope bias estimates for each of the N time steps where the smoother has
209209
been updated with measurements.
210210
"""
211-
if self.x.size == 0:
211+
x = self.x
212+
if x.size == 0:
212213
return np.empty((0, 3))
213214

214215
bg = self.x[:, 13:16]
@@ -224,12 +225,12 @@ def euler(self, degrees: bool = False) -> NDArray:
224225
Euler angles estimates for each of the N time steps where the smoother has
225226
been updated with measurements.
226227
"""
227-
theta = np.array([_euler_from_quaternion(q) for q in self.quaternion()])
228-
229-
if degrees:
230-
theta = (180.0 / np.pi) * theta
228+
q = self.quaternion()
229+
if q.size == 0:
230+
return np.empty((0, 3))
231231

232-
return theta
232+
theta = np.array([_euler_from_quaternion(q_i) for q_i in q])
233+
return np.degrees(theta) if degrees else theta
233234

234235
@staticmethod
235236
def _backward_sweep(

0 commit comments

Comments
 (0)