속도나 가속도 센서 등에서 데이터를 처음 읽어보면 생각보다 노이즈가 많은 것에 놀라곤 한다.
어쩌겠는가? 그 데이터라도 들어오는 것을 감사하고 써야지.
그래도 센서값을 잘 보면, 전체적인 경향성은 잘 맞는 것 같다.
노이즈를 제거하고 나면 꽤 쓸만할 것 같다는 생각이 든다.
자, 그럼 노이즈를 제거하는 예제를 하나 만들어보자.
가속도값에서 노이즈를 제거하는 예제를 만들건데,
예제를 보기 전에 일반적인 내용부터 한번 보자.
먼저, 보통 노이즈를 줄인다고 하면 아래와 같은 방법을 사용한다.
1) 저역통과 필터 (Low-pass Filter)
가장 간단하고 많이 쓰는 방법
예: 1차 IIR 필터
alpha = 0.1 # 0~1 사이 값 (작을수록 부드러움)
filtered_accel = alpha * measured_accel + (1 - alpha) * prev_filtered_accel
- 장점: 구현이 매우 간단
- 단점: 반응 속도가 느림 (딜레이)
2) 이동 평균 (Moving Average)
최근 N개의 측정값의 평균
window_size = 10
accel_window = []
def moving_average(new_accel):
accel_window.append(new_accel)
if len(accel_window) > window_size:
accel_window.pop(0)
return sum(accel_window) / len(accel_window)
- 장점: 간단하고 성능 좋음
- 단점: sudden change 반응이 늦어짐
3) 칼만 필터 (Kalman Filter)
속도/가속도 모두 함께 추정하면 특히 효과적
속도 데이터를 이용해 가속도를 추정하거나, 가속도+속도+위치 통합추정 가능
- 장점: 노이즈와 센서 바이어스 동시 보정 가능
- 단점: 수식 설정/튜닝이 필요
상태: x = [속도, 가속도]
관측: z = [IMU가속도]
모델:
x_k+1 = F x_k + w
z_k = H x_k + v
F, H, Q, R 설정
실제적인 예제는 아래와 같다.
자동차가 한 대 있다고 하자. 그 자동차에는 20ms 단위로 가속도값을 측정할 수 있는 IMU센서가 장착되어 있고, 차량의 현재 속도를 측정할 수 있는 휠스피드 센서가 있다고 하자.
즉, 우리는 차량의 현재 가속도를 측정하고자 하는데,
본 차량에는 이미 IMU 센서가 있어서 가속도를 바로 측정할 수 있다.
문제는 해당 센서에서 바로 읽어온 가속도는 전체적인 경향은 얻을 수 있으나, 하나 하나의 데이터는 노이즈가 많아서 신뢰하기 어려운 상황이다.
이때 노이즈를 제거해서 차량의 현재 가속도를 적당히 신뢰할 정도로 얻고자 하는 것이 목표이다.
1. 가장 쉬운 방법
가장 쉽게 low-pass filter 와 속도값으로부터 가속도를 유추하는 방법을 함께 사용해보자.
- IMU 가속도 → Low-pass filter로 노이즈 제거
- 휠스피드 → 속도 차분으로 가속도 계산
- 둘을 가중 평균 (α blending)
import numpy as np
# 제어 주기 (20ms)
dt = 0.02
# 필터 계수 (0~1 사이, 클수록 IMU값을 더 신뢰)
alpha = 0.6 # IMU 비중
beta = 1 - alpha # 휠스피드 차분 비중
# Low-pass filter 계수 (클수록 필터링 강함)
lpf_alpha = 0.2
# 초기값
prev_filtered_accel = 0.0
prev_wheel_speed = 0.0
def estimate_acceleration(imu_accel, wheel_speed):
global prev_filtered_accel, prev_wheel_speed
# 1. IMU 가속도 Low-pass filter 적용
filtered_imu_accel = lpf_alpha * imu_accel + (1 - lpf_alpha) * prev_filtered_accel
prev_filtered_accel = filtered_imu_accel
# 2. 휠스피드로 가속도 계산
accel_from_speed = (wheel_speed - prev_wheel_speed) / dt
prev_wheel_speed = wheel_speed
# 3. 두 값을 가중 평균으로 융합
fused_accel = alpha * filtered_imu_accel + beta * accel_from_speed
return fused_accel
# 예시 데이터 (IMU 가속도 값 [m/s^2], 휠스피드 [m/s])
imu_data = [0.1, 0.15, 0.3, 0.4, 0.2, 0.1, 0.05]
wheel_speed_data = [0.0, 0.02, 0.05, 0.09, 0.12, 0.14, 0.15]
for imu_a, ws in zip(imu_data, wheel_speed_data):
est_a = estimate_acceleration(imu_a, ws)
print(f"IMU: {imu_a:.2f} WheelSpeed: {ws:.2f} Estimated Accel: {est_a:.3f} m/s^2")
- lpf_alpha : IMU 노이즈 저감을 위한 low-pass filter 계수
- alpha : IMU 가속도와 휠스피드 차분값의 신뢰도 비율 (0~1)
- IMU 값이 믿을만하면 크게
- 휠스피드 차분이 안정적이면 작게
- 제어주기 20ms (dt = 0.02) 적용
- global 변수로 이전 값을 기억해 연결
2. 1차 칼만필터(1D constant acceleration model)
상태 변수
- x = [v, a]^T → 속도와 가속도
측정값
- z = [a_imu] (IMU 가속도만 사용)
- 휠스피드는 상태 v 갱신에 활용 (보완 가능)
# model
x_k+1 = F x_k + w_k
z_k = H x_k + v_k
# F: state transition matrix
F = [1 dt]
[0 1]
# H: observation matrix
H = [0 1] # IMU로 가속도만 측정 가능
아래는 파이썬으로 구현한 1차 칼만필터 코드이다.
import numpy as np
# 제어 주기
dt = 0.02 # 20ms
# 상태전이 행렬 (F)
F = np.array([[1, dt],
[0, 1]])
# 관측행렬 (H) → IMU 가속도만 관측
H = np.array([[0, 1]])
# 공분산 행렬 초기값 (P)
P = np.eye(2)
# 프로세스 노이즈 (Q)
q_v = 0.01 # 속도 노이즈 분산
q_a = 0.1 # 가속도 노이즈 분산
Q = np.array([[q_v, 0],
[0, q_a]])
# 측정 노이즈 (R) → IMU 가속도 측정 노이즈 분산
r_a = 0.5
R = np.array([[r_a]])
# 초기 상태 [속도, 가속도]
x = np.array([[0.0],
[0.0]])
def kalman_filter(z_measured):
global x, P
# 1. 예측 (Prediction)
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# 2. 칼만 이득 계산
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
# 3. 상태 갱신 (Update)
y = np.array([[z_measured]]) - (H @ x_pred)
x = x_pred + K @ y
# 4. 공분산 갱신
P = (np.eye(2) - K @ H) @ P_pred
return x[0, 0], x[1, 0] # 속도, 가속도
# 예시 데이터 (IMU 가속도 [m/s^2])
imu_data = [0.1, 0.15, 0.3, 0.4, 0.2, 0.1, 0.05]
for imu_a in imu_data:
v_est, a_est = kalman_filter(imu_a)
print(f"IMU: {imu_a:.2f} Estimated Speed: {v_est:.3f} m/s Estimated Accel: {a_est:.3f} m/s^2")
위의 예제에서는 IMU 값만 입력으로 하는 칼만필터 였다.
그런데 우리는 휠스피드 센서도 있기 때문에 그것도 가속도 노이즈 제거에 사용할 수 있다.
그렇게 하기 위해서는 2센서 칼만필터로 변경해야 한다.
3. 2차 칼만필터(2D constant acceleration model)
칼만필터를 사용하기 위해서는 상태(state), 측정값(센서값), 모델 등을 정의해야 하는데,
아래 코드에는 해당 내용이 모두 포함되어 있다.
# x: state
x = [v, a]^T
# z: observation
z = [v_wheel, a_imu]^T
# model
x_k+1 = F x_k + w_k
z_k = H x_k + v_k
# F: state transition matrix
F = [1 dt]
[0 1]
# H: observation matrix
H = [1 0] # 휠스피드 (속도)
[0 1] # IMU 가속도
전체 파이썬 코드는 아래와 같다.
import numpy as np
# 제어 주기
dt = 0.02 # 20ms
# 상태전이 행렬 (F)
F = np.array([[1, dt],
[0, 1]])
# 관측행렬 (H) → 휠스피드 속도 + IMU 가속도 관측
H = np.array([[1, 0],
[0, 1]])
# 프로세스 노이즈 공분산 (Q)
q_v = 0.01 # 속도 노이즈 분산
q_a = 0.1 # 가속도 노이즈 분산
Q = np.array([[q_v, 0],
[0, q_a]])
# 측정 노이즈 공분산 (R)
r_v = 0.05 # 휠스피드 노이즈
r_a = 0.5 # IMU 가속도 노이즈
R = np.array([[r_v, 0],
[0, r_a]])
# 초기 상태 [속도, 가속도]
x = np.array([[0.0],
[0.0]])
# 공분산 행렬 초기값 (P)
P = np.eye(2)
def kalman_filter(z_measured):
global x, P
# 1. 예측 (Prediction)
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# 2. 칼만 이득 계산
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
# 3. 상태 갱신 (Update)
y = z_measured.reshape(-1,1) - (H @ x_pred)
x = x_pred + K @ y
# 4. 공분산 갱신
P = (np.eye(2) - K @ H) @ P_pred
return x[0, 0], x[1, 0] # 속도, 가속도
# 예시 데이터 (휠스피드 [m/s], IMU 가속도 [m/s^2])
wheel_speed_data = [0.0, 0.02, 0.05, 0.09, 0.12, 0.14, 0.15]
imu_data = [0.1, 0.15, 0.3, 0.4, 0.2, 0.1, 0.05]
for ws, imu_a in zip(wheel_speed_data, imu_data):
z_measured = np.array([ws, imu_a])
v_est, a_est = kalman_filter(z_measured)
print(f"WheelSpeed: {ws:.2f} IMU: {imu_a:.2f} | Estimated Speed: {v_est:.3f} m/s Estimated Accel: {a_est:.3f} m/s^2")
그러나 위의 방법을 사용하더라도 일정 수준의 delay가 발생하는 것은 어쩔 수 없다.
이런 delay를 보상하기 위해 forward prediction 까지 수행하기도 한다.
일단은 noise 제거까지만 수행해보고, 다음에 forward prediction 까지 해보도록 하자.
그럼 오늘은 이만~
'Math' 카테고리의 다른 글
| 한 점에서부터 곡선까지의 최단거리 (1) | 2025.03.24 |
|---|---|
| 평면에서의 좌표 회전 vs 좌표축 회전(Rotation) (0) | 2025.03.18 |
