8/22 - gps 위도 경도 와 차량 정보를 이용해서 계산하기

 

한국에서 위도 경도를 이용해서 x,y값을 계산하고 칼만 필터를 이용해서 글로벌 좌표계로 변환하면 될것 같다.

 

대한민국에서 GPS 위도와 경도를 평면 좌표계의 x, y 좌표로 변환하려면, 보통 UTM (Universal Transverse Mercator) 좌표계 또는 TM (Transverse Mercator) 좌표계를 사용합니다. 이 방법은 구면인 지구의 표면을 평면으로 투영하는 방법 중 하나입니다.

여기서는 대한민국에 적합한 Transverse Mercator 좌표계인 TM128 좌표계로 변환하는 방법을 설명하겠습니다. 대한민국은 EPSG:5179 (Korean 2000 / Korea Central Belt TM) 좌표계를 사용합니다.

1. EPSG:5179 좌표계

• 중앙 경선: 127도
• 기준 위도: 38도
• 단위: 미터 (x, y 좌표는 미터 단위로 표현됩니다)

2. 파이썬에서 좌표 변환 수행

파이썬에서는 pyproj 라이브러리를 사용하여 위도, 경도를 x, y 좌표로 변환할 수 있습니다.

1. 필요한 라이브러리 설치:

pip install pyproj

2. 좌표 변환 코드:

from pyproj import Proj, transform

# EPSG:4326 (WGS84) 좌표계 - 일반적인 GPS 위도/경도 좌표계
wgs84 = Proj(init='epsg:4326')

# EPSG:5179 (TM128) 좌표계 - 대한민국 TM128 좌표계
tm128 = Proj(init='epsg:5179')

# 변환할 위도/경도 (예: 서울시청)
lat, lon = 37.5665, 126.9780

# WGS84에서 TM128로 변환
x, y = transform(wgs84, tm128, lon, lat)

print(f"x: {x}, y: {y}")

3. 예시:

서울시청의 위도와 경도는 (37.5665, 126.9780)입니다. 이를 TM128 좌표계로 변환하면 대략적으로 다음과 같은 x, y 값을 얻을 수 있습니다:

x: 199961.2855
y: 451113.5977

이 값들은 서울시청의 위치를 미터 단위로 나타낸 것으로, 대한민국의 TM128 좌표계에서의 위치입니다.

4. 결론:

위와 같은 방법을 통해 대한민국 내의 GPS 위도, 경도를 x, y 좌표로 변환할 수 있습니다. 이 과정은 지리 정보를 다루는 다양한 애플리케이션에서 널리 사용됩니다. UTM이나 TM 좌표계는 특정 지역에 최적화되어 있어, 이와 같은 방법이 대한민국 내에서의 변환에 적합합니다.

---

위도 경도로 x,y값을 구했으면 아래 코드로 구하면 될것 같다.

 

차량 정보로 vehicle speed(m/s)와 yaw rate(rad/s)를 받아서 칼만 필터에 적용하는 방법은 차량의 운동 모델을 활용하여 상태를 예측하고, GPS 데이터와 결합하여 상태를 갱신하는 방식으로 이루어집니다.

이 경우, 칼만 필터의 상태 벡터는 차량의 위치와 속도를 포함하며, yaw rate와 vehicle speed를 이용하여 예측 단계를 수행합니다.

1. 상태 벡터 및 모델 설정

우선, 상태 벡터는 다음과 같이 정의할 수 있습니다:

2. 상태 전이 모델 (State Transition Model)

상태 전이 모델은 차량의 현재 상태와 vehicle speed, yaw rate를 바탕으로 다음 상태를 예측하는 데 사용됩니다. 여기서는 차량의 동역학 모델을 사용하여 다음과 같이 상태를 예측할 수 있습니다:

3. 칼만 필터 예측 단계

예측 단계에서는 차량의 현재 상태와 제어 입력(속도, yaw rate)을 사용하여 다음 상태를 예측합니다.

4. 칼만 필터 갱신 단계

갱신 단계에서는 GPS로부터 얻은 실제 측정치를 사용하여 예측된 상태를 수정합니다.

5. 파이썬 코드 예시

import numpy as np

class KalmanFilter:
    def __init__(self, dt):
        self.dt = dt  # Time step
        
        # 초기 상태 벡터 [x, y, theta, v]
        self.x = np.zeros(4)  
        
        # 초기 상태 오차 공분산 행렬
        self.P = np.eye(4)  
        
        # 상태 전이 행렬 A
        self.A = np.eye(4)
        
        # 제어 입력 행렬 B
        self.B = np.zeros((4, 2))
        
        # 프로세스 노이즈 공분산 행렬 Q
        self.Q = np.eye(4) * 0.01
        
        # 측정 행렬 H (GPS 측정은 위치 정보만 제공하므로)
        self.H = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])
        
        # 측정 노이즈 공분산 행렬 R
        self.R = np.eye(2) * 0.1
        
    def predict(self, u):
        theta = self.x[2]
        v = self.x[3]
        
        # 상태 전이 행렬 A 업데이트
        self.A[0, 2] = -v * self.dt * np.sin(theta)
        self.A[0, 3] = self.dt * np.cos(theta)
        self.A[1, 2] = v * self.dt * np.cos(theta)
        self.A[1, 3] = self.dt * np.sin(theta)
        
        # 제어 입력 행렬 B 업데이트
        self.B[0, 0] = 0.5 * self.dt ** 2 * np.cos(theta)
        self.B[1, 0] = 0.5 * self.dt ** 2 * np.sin(theta)
        self.B[2, 1] = self.dt
        self.B[3, 0] = self.dt
        
        # 상태 예측
        self.x = np.dot(self.A, self.x) + np.dot(self.B, u)
        
        # 오차 공분산 예측
        self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q

    def update(self, z):
        # 측정 잔차 계산
        y = z - np.dot(self.H, self.x)
        
        # 잔차 공분산 계산
        S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
        
        # 칼만 이득 계산
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        
        # 상태 업데이트
        self.x = self.x + np.dot(K, y)
        
        # 오차 공분산 업데이트
        I = np.eye(4)
        self.P = np.dot(I - np.dot(K, self.H), self.P)

# 사용 예시
dt = 0.1  # 시간 간격 (초)
kf = KalmanFilter(dt)

# 차량 속도와 yaw-rate 제어 입력
vehicle_speed = 15.0  # m/s
yaw_rate = 0.01  # rad/s
u = np.array([vehicle_speed, yaw_rate])

# 예측 단계 (차량의 상태 예측)
kf.predict(u)

# GPS 측정치 업데이트
gps_measurement = np.array([100.0, 200.0])
kf.update(gps_measurement)

print(f"추정된 상태: {kf.x}")

6. 설명

• 상태 벡터 (self.x): 차량의 현재 상태를 나타내며, 위치 (x, y), 방향 (θ\thetaθ), 속도 (v)를 포함합니다.
• 예측 단계 (predict): 차량의 동역학 모델과 주어진 제어 입력(속도와 yaw-rate)을 사용하여 차량의 다음 상태를 예측합니다.
• 갱신 단계 (update): GPS 측정치와 예측된 상태를 비교하여 상태를 갱신합니다.

7. 결론

이러한 방법으로, 차량의 vehicle speed와 yaw rate를 활용하여 칼만 필터를 구성할 수 있으며, GPS 측정치와 결합하여 더 정확한 위치 추정을 할 수 있습니다. 이 방법은 특히 GPS 신호가 불안정하거나 간헐적으로 끊기는 상황에서 유용합니다.