zumi - 25. mpu6050 가속도계로 속도, 위치 구하기 fail

mpu6050은 관성 센서인데

 

가속도계도 들어있으니

 

가속도로 속도를 구하고, 속도로 위치를 구할수 있을거라 생각했다.

 

 

 

 

그래서 우선 x축 가속도 값을 출력해보기도 하고

 

from zumi.zumi import Zumi
import time
import datetime as pydatetime

def get_now():
    return pydatetime.datetime.now().timestamp()

#initialize
zumi = Zumi()


bf_time = 0
curr_time = 0

def get_offset(zumi, sampling):
    offset = 0
    for i in range(1, sampling):
        acc = zumi.get_acc()
        accx = round(acc[0], 3)
        offset = offset + accx
    
    offset = offset/sampling
    print("offset : " + str(offset))
    return offset

def print_acc(zumi, offset):
    global curr_time, bf_time
    acc = zumi.get_acc()
    acc_x = round(acc[0] - offset, 2)
    curr_time = get_now()
    dt = round(curr_time - bf_time, 3)
    msg = "acc x : "+str(acc_x)  + ", dt : " + str(dt)
    bf_time = curr_time
    print(msg)

def do_something(zumi, offset):
    print_acc(zumi, offset)
    time.sleep(0.1)



try:
    offset = get_offset(zumi, 100)
    while True:
        do_something(zumi, offset)
except KeyboardInterrupt:
    pass

 

 

 

센서 데이터 특정 시간을 타임 스탬프로 출력해서

 

시간 구간으로 적분해보기도 했다.

 

더해서 LPF도 써보기는 했으나

 

 

from zumi.zumi import Zumi
import time
import datetime as pydatetime

def get_now():
    return pydatetime.datetime.now().timestamp()

#initialize
zumi = Zumi()
vel = 0
alpha = 0.7
bf_time = 0
curr_time = 0
prev_acc = 0
def get_offset(zumi, sampling):
    offset = 0
    for i in range(1, sampling):
        acc = zumi.get_acc()
        accx = round(acc[0], 3)
        offset = offset + accx
    
    offset = offset/sampling
    print("offset : " + str(offset))
    return offset


def LPF(alpha, prev, curr):
    val = alpha * prev + (1-alpha) * curr
    return val

def print_acc(zumi, offset):
    global curr_time, bf_time, vel, prev_acc
    acc = zumi.get_acc()
    acc_x = LPF(alpha, prev_acc,acc[0])
    acc_x = round(acc[0] - offset, 2)

    curr_time = get_now()
    dt = round(curr_time - bf_time, 2)
    vel = vel + acc_x * dt
    vel = round(vel, 2)
    msg = "acc x : "+str(acc_x)  + ", vel : " +str(vel)+", dt : " + str(dt)
    bf_time = curr_time
    prev_acc = acc_x
    print(msg)

def do_something(zumi, offset):
    print_acc(zumi, offset)
    time.sleep(0.1)



try:
    offset = get_offset(zumi, 100)
    bf_time = get_now()
    while True:
        do_something(zumi, offset)
except KeyboardInterrupt:
    pass

 

 

가속도계 자체 오차가 너무 심해서 

 

처음 초기화 당시에는 안정적이더라도

 

조금만 움직이고 나서는 오차가 점점 누적되어 발산하게 되버리더라

 

 

 

내 딴에는 나름대로는 칼만필터도 써보긴했는데 

 

 

 

과정은 따로 녹화는 안해서 올리진 못한게 아쉬워도 여기까지 하고

 

 

 

그래서 mpu6050에서 가속도계는

 

자이로 센서로 각 변화율을 구하는데 보조해주기만 하나보더라