최적 제어를 위해 우리는 지금까지 space-state model을 구축하고, non-linear system을 linearization하고, Pole Placement와 LQR을 적용하고 있습니다. 하지만, 이 모든 과정들에서 가정하고 있던 것이 있는데요. 바로 system의 state를 완벽하게 알고 있어야 한다는 점입니다.

위치, 속도, 가속도와 같은 state는 센서를 통해 유추해야 하며, 모든 state를 측정하기 위한 시스템은 매우 비용이 높기 때문에 속도를 적분하여 위치를 얻거나, 위치를 미분하여 속도를 얻는 등, estimation을 주로 사용합니다. 따라서 이 estimation이 얼마나 정확한지도 제어의 큰 요소가 됩니다.

• 전체 state와, 센서를 통해 measurement가 가능한 state들 사이의 관계는 아래와 같이 Matrix 형태로 표현할 수 있습니다. 속도를 측정하여 위치를 추론하는 상황은 아래와 같이 표현할 수 있지요.

• measurement와 estimation을 통해 구성한 state는 실제 로봇 state와 일치하지 않아 error를 갖습니다. 이 error를 최소화하는 estimator를 구현하는 것이 또 우리의 목표가 되는 것입니다. 문제의 해결을 위해 이를 수식으로 표현해보겠습니다.

결국 A matrix의 eigenvalue에 따라 error의 수렴/발산/진동 여부가 결정되고, Control 시 pole placement를 사용한 것처럼 이번에도 무언가의 조작을 통해 이 A martix를 수정해줘야 합니다.

따라서 이번 시간에는 두가지 state estimator에 대해 살펴보고, 시뮬레이션해보겠습니다.

1. Luenberger Observer (deterministic estimator)
2. Kalman Filter (stochastic estimator)

deterministic이란 말은, 같은 input을 가질 때 항상 같은 output을 얻는다는 뜻입니다. 반면, stochastic은 확률에 기반하기 때문에 같은 input일지라도 다른 output을 받게 됩니다.

Luenberger Observer

Luenberger Observer는 pole placement와 매우 유사하며, 대신 control stability를 만족하기 위함이 아니라, estimation stability를 만족시키기 위한 state observer입니다.

• Gain L과 measurement error를 사용하여 estimate state $\hat{x}$을 아래와 같이 정의합니다.

• 새롭게 정의한 $\hat{x}$을 사용하여 전체 error를 정리하면, L,C를 통해 A matrix를 조작할 수 있는 형태가 됨을 알 수 있습니다. 이전 pole placement시 B, K를 사용했던 것과 완전 동일한 형태임을 할 수 있지요.

• 이제 stability를 만족하는 Gain L을 찾아내야 하는데요. 계산 과정은 직접 유도하지 않고 python의 control 패키지를 통해 손쉽게 구해보겠습니다.

잠시 Observer Gain 구현이 왜 이렇게 사용되는지, control과 estimate 사이의 어떠한 관계를 갖는지 이해하는 시간을 가져보겠습니다.

• Observability와 Controllability의 판별식을 잘 살펴보면 공통된 형태를 갖는 것을 알 수 있습니다.

• Controllability의 A 대신 A transpose가, B 대신 C transpose가 사용된 점을 확인 가능하며, 따라서 Controllability를 판별하는 python 함수를 그대로 사용하여 Observability도 판별할 수 있습니다.

• 동일한 맥락으로, control의 pole placement 시 A, B Matrix와 Pole p를 통해 원하는 Gain K를 계산했던 것처럼, Observability Gain L 계산을 위해 A, C transpose와 Pole p을 사용하면 됩니다.

코드 구현

첫번째 코드에서는 Controllability와 Observability 사이의 관계, 그리고 Luenberger Observer를 통한 estimation gain L 계산을 실습해보겠습니다.

• spring-mass system의 dynamics를 구현하고 이번에는 추가로 observe matrix C가 추가되었습니다. 두 mass의 velocity를 관측하게 될 것입니다.

• 첫번째 코드는, 시스템의 observability stability를 확인합니다. 결과에서 알 수 있듯이 복소항이 존재하여 진동하는 결과를 예상할 수 있지요.

• 다음으로, observability를 판단해보겠습니다. control.obsv(A,C)를 통해 계산한 결과와, control.ctrb(A.T, C.T)를 통해 계산한 결과의 transpose가 동일한 것이 확인됩니다.

• pole placement 시 사용했던 control.place 함수를 사용하되, A,B 대신 A.T, C.T를 사용하면 Luenberger Observer를 구현할 수 있습니다.

• 새롭게 구한 L matrix를 통해 system의 observabilty를 다시 계산해봅시다.

eigenvalue가 원하는 위치로 변경된 것을 알 수 있습니다! 그럼 이번에는 시뮬레이션을 통해 실제 얼마나 정확한 estimation이 가능해졌는지 살펴봅시다.

• 일전, estimate state $\hat{x}$은 아래와 같이 모델링 한 바 있습니다. 원래 control u에 대해서도 고려해야 하지만, 이번 코드에서는 estimator의 성능만을 살펴보기 위해 외부 control은 전혀 없는 상황을 가정하겠습니다.

• odeint의 input이 되는 dynamics 함수 spring_mass_dynamics는 다음과 같습니다. np.block을 통해 8*8 matrix를 구성한 이유는 정확한 state x와 estimated state $\hat{x}$을 구분하기 위함입니다.

💡우선, estimator를 구현하지 않고 시뮬레이션한 결과를 보기 위해 아래와 같이 Abig을 설정하였습니다.

• 방금 전, control.place 함수를 통해 구해낸 matrix L을 사용하여 시뮬레이션을 실행해봅시다.

• 아직 estimator를 적용하지 않아 estimator는 오로지 initial state에만 의존하게 됩니다.

• 따라서, initial state를 모두 0으로 만들어버리면 아래와 같은 결과를 얻습니다.

x0_est  = np.array([0.0, 0.0, 0.0, 0.0])

• 이번에는 Luenberger Observer를 적용하여 다시 시뮬레이션 해봅시다.

⇒ 일전보다는 확실히 estimation이 적용되고 있음을 확인할 수 있습니다. L Gain의 tuning을 위해선 pole의 위치를 바꿔줘야겠지요? 그런데 어떠한 pole이 더 적합할지는 또 어떻게 알 수 있을까요? 이러한 문제들을 해결할 수 있는 또다른 estimator인 Kalman Filter에 대해 배워봅시다.

Kalman Filter

Luenberger Observer는 same input / same output을 갖는 “deterministic method” 임에 비해, Kalman Filter는 same input이어도 다른 output을 얻을 수 있는 “stochastic method”입니다.

• Kalman Filter (혹은 Linear Quadratics Estimator)는 process noise와 measurement noise를 포함하여 시스템을 다음과 같이 모델링합니다.

• process noise란 제어 u에 추가되는 noise이며, measurement noise란 state y를 계산하면서 추가되는 noise 입니다. Kalman Filter는 이 noise들에 기반하여 아래와 같이 계산되는 Kalman Gain을 통해 최종 estimate gain L을 도출합니다.

• estimated state $\hat{x}$을 구하고, estimate gain L을 구하는 방법은 다음과 같습니다. estimate gain L은 LQR에서 언급되었던 것과 유사하게 Ricattic 방정식의 풀이로 구해지며, 이번 강의에서 직접 풀이를 하지 않고 대신 python의 control 패키지를 사용하여 손쉽게 구할 것입니다.

코드 구현

• 계속해서 사용하던 dynamics인 spring-mass system에 estimation을 적용해보겠습니다. control 없이 본연 그대로의 state estimation을 진행하는 것도 동일하며, 대신 process noise와 곱해지는 matrix G를 선언하였습니다. (속도에 대해 noise를 적용하였습니다.)

• Noise term Qe와 Re도 임의의 값으로 추가한 뒤 control.lqe를 사용하여 estimate gain L을 계산한 모습입니다. (P는 Ricatti Equation의 중간 결과이며, E는 Gain이 적용된 시스템에서의 eigenvalue를 나타냅니다.)

이 시점에서 혼란을 막기 위해 행렬들을 정리하고 넘어가겠습니다.

• odeint의 input이 되는 함수인 spring_mass_dynamics에서 Gain L을 사용한 estimation을 구현합니다.

실제 simulation value와 estimation state를 한번에 계산하기 위해 행렬 계산식이 살짝 복잡해졌지만, 아래 수식을 보시면 쉽게 이해할 수 있을 것입니다.

• for-loop를 순회하며 odeint를 통해 다음 state를 계산하는 과정은 일전 예시들과 완전 동일합니다. 대신, 매 step별 적용하는 noise를 random하게 해주는 코드가 삽입되었습니다.

• 작성한 코드를 실행해봅시다. noise가 전혀 없는 actual state와, process/measurement noise를 포함하는 estimate state를 확인할 수 있습니다. 시뮬레이션 초반에는 다소 오차를 보이지만, 점차 시간이 지날수록 실제 state에 수렴해가는 모습을 확인할 수 있습니다.

Exercise

• 아래과 같이 다양한 noise를 적용하여 코드를 다시 실행하고 결과를 분석해보세요.

• 위치-속도-가속도 사이 non-linear 상관관계를 갖는 시스템 (joint angle 등)에서는 어떻게 estimation할 수 있을까요? 지금까지 배운 내용들을 종합하여 생각해봅시다.

• 실제 로봇 시스템에서 process nosie와 measurement noise는 어떻게 설정해야 할지 조사해봅시다.