5. 상태 방정식 (State Equation)

상태 방정식 (State Equation)

✔ 이전 상태(위치 등)와 현재 상태 사이의 관계를 나타냄

✔ 측정 방정식(measurement equation)이란 현재 상태와 센서 데이터 사이의 관계

✔ 1차원에서 직선으로 움직이는 어떤 물체의 움직임의 상태를 표현하는 상태 방정식을 다룸

상태 방정식 전개

상태 방정식 최종형태

$$\begin{array}{rl}& x(k)=Ax(k-1)+B(u(k)+w(k))\\ & \\ & z(k)=Cx(k)+v(k)\end{array}$$

상태 방정식 유도

$$\begin{array}{rl}& x(k)=x(k-1)+T\dot{x}(k-1)+\frac{T^2}{2}\ddot{x}(k-1)+\frac{T^2}{2}(u(k)+w(k))\\ & \dot{x}(k)=\dot{x}(k-1)+T\dot{x}(k-1)+T(u(k)+w(k))\\ & \ddot{x}(k)=\ddot{x}(k-1)+(u(k)+w(k))\\ & z(k)=x(k)+v(k)\end{array}$$

• $k:$ step

• $x(k):$ k 번째 step의 물체의 위치 상태

• $\dot{x}(k):$ k 번째 step의 물체의 속도

• $\ddot{x}(k):$ k 번째 step의 물체의 가속도

• $T:$ 각 step 간 시간 간격

• 기본적으로 등가속도운동 공식에 따라 현재 위치 $x(k$)는 다음과 같이 정의할 수 있음

$$\begin{array}{rl}\text{현재위치}& =\text{이전위치}+\text{이동거리}\\ & \\ \text{현재위치}& =\text{이전위치}+\frac{\text{이전 속도}+\text{현재 속도}}{2}\times \text{시간}\\ & \\ x(k)& =x(k-1)+\frac{\dot{x}(k-1)+\dot{x}(k)}{2}\times T\\ & \end{array}$$

• 현재 속도 $\dot{x}(k)$ 는 다음과 같이 정의할 수 있음

$$\begin{array}{rl}\text{현재속도}& =\text{이전속도}+\text{가속도}\times \text{시간}\\ & \\ \dot{x}(k)& =\dot{x}(k-1)+\ddot{x}(k)\times T\\ & \end{array}$$

• 현재 가속도 $\ddot{x}(k)$ 는 **입력 제어 값 $u(k)$과 노이즈$w(k)$**을 이용해 다음과 같이 정의할 수 있음 (엑셀레이터와 같은 입력으로 인해 가속도가 바뀜)

$$\begin{array}{r}\ddot{x}(k)=\ddot{x}(k-1)+(u(k)+w(k))\\ \end{array}$$

• ==따라서, 위의 식들을 이용하여 현재 상태 $x(k)$ 에 대한 식을 풀어보면,==

$$\begin{array}{rl}x(k)& =x(k-1)+\frac{\dot{x}(k-1)+\dot{x}(k)}{2}\times T\\ & \\ & =x(k-1)+\frac{T}{2}\dot{x}(k-1)+\frac{T}{2}\dot{x}(k)\\ & \\ & =x(k-1)+\frac{T}{2}\dot{x}(k-1)+\frac{T}{2}(\dot{x}(k-1)+\ddot{x}(k)\times T)\\ & \\ & =x(k-1)+T\dot{x}(k-1)+\frac{T^2}{2}\ddot{x}(k)\\ & \\ & =x(k-1)+T\dot{x}(k-1)+\frac{T^2}{2}(\ddot{x}(k-1)+(u(k)+w(k)))\\ & \\ & =x(k-1)+T\dot{x}(k-1)+\frac{T^2}{2}\ddot{x}(k-1)+\frac{T^2}{2}(u(k)+w(k))\\ & \end{array}$$

측정 방정식

• 위치를 가리키는 센서 데이터 $z(k)$와 위치 상태 $x(k)$의 관계를 나타내는 측정 방정식(measurement equation) 은 다음과 같이 표현함

$$z(k)=x(k)+v(k)$$

• 이때, $v(k)$는 센서 자체의 노이즈(분산)을 의미함
• 센서의 노이즈가 작으면 왼쪽과 같이 물체를 인식한 분포가 좁은 영역에 겹쳐져 있고, 이는 노이즈(분산)이 작다고 말할 수 있음
• 이 노이즈 때문에 항상 $z(k)=x(k)$ 일 수 없음

상태방정식과 측정방정식 행렬식

• 상태 방정식을 연립방정식의 행렬표현에 따라 행렬로 나타내면 위와 같음
• 위치 상태인 $x(k)$를 3차원 (위치, 속도, 가속도) 로 보고, 이전 상태 $x(k-1)$의 계수 행렬 A를 state matrix라고 함
• 입력 제어 값 $(u(k)+w(k))$에 대한 계수 행렬 B를 control matrix라고 함

• 위 측정방정식의 행렬 C를 transition matrix 라고 함
• 따라서 상태방정식과 측정방정식의 최종 형태는 다음과 같음

& x(k) = Ax(k-1) + B(u(k) + w(k)) \\\\ & z(k) = Cx(k) + v(k) \end{align}

정리

• 상태 방정식에서 제어 입력에 대한 노이즈는 $w(k)$와 측정 방정식에서 측정값에 대한 노이즈는 $v(k)$는 Localization과 Tracking 문제를 모델링 할 때 매우 중요한 역할을 함
• Localization 문제에서는 제어 입력 $(u(k)+w(k))$이 입력되는 반면, Tracking 문제에서는 제어 입력이 주어지지 않으므로 더 어려움

---

참고

• 상태 방정식 (state equation) - gaussian37

---

Prev: 4. Localization과 Tracking

Next: 6. 베이즈 필터(Bayes Filter)

---

상태추정상태방정식