우리가 사용하는 센서들은 노이즈가 존재하며, 우리가 사는 곳은 데이터로 가득합니다. 우리는 센서로 측정하고 추적을 하고 싶지만 센서는 노이즈로 인해 완벽한 정보를 주지는 않습니다. 자동차에 GPS가 장착됬을때를 생각해보면 도로위에 같은 지점에 서있더라도 gps는 조금씩 다른 위치에 있다고 알려줄 겁니다.
이런 단순한 상황만 봐도 해결방법은 간단합니다. 읽은 데이터로 부터 서로다른 가중치를 주고 그들의 평균을 구하면 정확한 값을 구하게 됩니다. 하지만 이 센서의 노이즈가 아주 심하다거나 데이터를 수집하기 힘든 환경인 경우 어떻게 해야할까요? 날아가는 비행체의 위치를 추적한다고 하거나 드론에 사용할 오토파일럿을 만드는 경우 들도 생각해봅시다. 저는 컴퓨터 비전을 연구하다보니 영상으로 움직이는 물체를 추적해야 합니다. 하지만 컴퓨터 비전 알고리즘들은 노이즈가 심하고 신뢰할수 없는 결과를 만들어내요.
이 책은 이러한 필터 문제들을 푸는 방법들을 알려줄겁니다. 저는 다양한 알고리즘들을 사용할거지만 모두 베이지안 확률 bayesian probability에 기반합니다. 베이지안 확률을 간단히 정리하자면 이전의 정보를 이용해서 실제와 비슷한 값을 구하는데 사용되비다.
제가 당신에게 지금 차의 머리 방향을 묻는다면 어떻게 대답해야 할지 모를겁니다. 아마 1도에서 360도 사이의 수가 되겠지만, 2초 전에 차의 해딩이 243도 이고, 2초가 지난후에도 차가 회전하지 않았다면 이전 보다는 신뢰할수 있는 예측값을 구할수 있을 겁니다. 이런식으로 이전 정보로 현재나 미래의 정보를 보다 정확하게 추정을 할수 있게 됩니다.
여기에 베이지안 확률이 사용됩니다. 중요한 개념으로 불확실성 uncertain이 있으며, 측정 값을 얼마나 신뢰 할수 있는지에 따라서 신뢰도를 바꾸어 갈겁니다. 칼만 필터와 베이지안 필터는 노이즈와 시스템의 동작 그리고 센서 값을 사용해서 가장 신뢰할수 있는 시스템의 상태를 추정해 나갑니다.
이제 우리는 움직이는 물체를 추적하는 중이고, 센서가 갑자기 물체의 방향이 바뀌었다고 알려줍니다. 실제로 회전이 발생한 걸가요? 그 데이터가 노이즈를 가지고 있는 걸가요? 이는 시스템에 따라 고려해볼수 있는데 만약 이물체가 제트 파이터라면 갑자기 조종해서 바꾼 것이라고 볼수 있지만, 일직선으로 지나가는 기차인 경우에는 노이즈를 무시해도 될겁니다. 더 나아가 우리는 센서의 정확도에 따라서 신뢰도를 바꿀수 있으며, 이 신뢰도는 이전 정보와 우리가 알고있는 시스템에 대한 정보 그리고 센서의 특성에 의존하게 될겁니다.
칼만 필터는 루돌프 에밀 칼만이 최적화 문제를 수학적으로 풀기 위해 만들었으며 달에 갈때 아폴로 미션에서 처음 사용되었습니다. 그리고 이 기술은 현제 수많은 분야에서 사용되고 있습니다. 칼만 필터는 비행체나 잠수함, 크루즈 미사일 등에서 사용되고 있으며 월 스트리트 역시 시장을 예측하는데도 사용하며 로봇, IoT 등 다양한 환경에서 사용되는 중입니다.
신뢰도는 상태 $x_t$로 나타내며, 이 값은 모든 이전 상태와 측정, 그리고 제어 입력이 주어질 때 구할수 있는데, 이 조건부 확률은 수학적으로 정리하면 다음과 같습니다.
1. $z_t$는 측정
2. $u_t$ 동작 명령
3. $x_t$는 시간 t에 대한 로봇이나 주위 환경의 상태(위치, 속도 등)
만약 우리가 상태 $x_{t-1}$과 $u_t$를 알고 있다면, 상태 $x_{0:t-2}$와 $z_{1:t-1}$는 조건부 독립 성질에 따라 아는게 크게 중요하지는 않습니다. 상태 $x_{t-1}$는 $x_t$와 $z_{1:t-1}$, $u_{1:t1}$ 사이 조건부 독립을 나타내는데 사용됩니다. 이를 정리하면 다음과 같습니다.
2법칙
$x_t$가 완전하다면
여기서 $x_t$가 완전하다는것의 의미는 과거의 상태, 측정, 동작 명령이 미래를 예측하는데 필요한 정보를 주지않는 경우를 말합니다.
p($x_t$ | $x_{t-1}$, $u_t$) -> 상태 전이 확률 state trasition probability
p($z_t$ | $x_t$) -> 측정 확률
조건부 의존과 조건부 독립의 예제
- 독립인 경우와 조건부 의존하는 경우
2개의 양면 동전이 있다고 합시다.
A - 첫번째 동전이 앞면을
B - 두 번째 동전이 앞면을
C - 두 동전이 같은 경우
A와 B만 보면 독립이지만, C가 주어진 경우라면 A와 B는 조건부 독립이 됩니다. C라는 사실을 알고 있다면 첫번째 동전의 결과가 다른 것의 결과도 알려주기 때문입니다.
- 의존하는 경우와 조건부 독립인 경우
어느 상자에 2개의 동전이 있는데 1개는 일반적인 동전이고 하나는 둘다 동일한 가짜 동전이라고 합시다(P(H) = 1). 동전을 하나 고르고 던지는 것을 2번 해보면 이 사건들을 다음과 같이 정의할 수 있습니다.
A = 첫 코인이 앞면이 나온 경우
B = 두 번째 동전이 앞면이 나오는 경우
C = 일반 동전을 골른 경우
A가 발생한 경우, 우리가 일반 코인 보다는 가짜 코인을 고른 가능성이 더 크다고 볼 수 있습니다. 이는 B가 발생할 조건부 확률을 증가시키는데, A와 B는 의존 관계임을 의미합니다. 하지만 C라는 사실이 주어질 때는 A와 B는 서로 독립이 됩니다.
베이즈 법칙 bayes rule
사후 확률 posterior P(B | A) = $\frac{우도 Likelihood P(A|B) * 사전 확률 prior P(B)} {주변 확률 분포 Marginal P(A)}$
여기서
- 사후 확률 : A라는 사건 일어났을때, 사건 B가 일어날 확률
- 우도 : 사건 B가 일어났을때, A인 확률
- 사전 확률 : 사건 B가 일어날 확률
- 주변 확률 : 사건 A가 일어날 확률
예제 :
여성 중 1%는 유방암을 가지고 99%는 그렇지 않습니다. 유방함 검사로 80%의 경우 유방암을 찾아내지만 20%는 놓치게 됩니다. 유방암 검사에서 9.6%는 유방암이 존재하지 않지만 유방암이 있다고 잘못 찾아냅니다.
베이즈 정리로 이를 식으로 정리하면 다음과 같이 정리할 수 있습니다.
- Pr(A|X) - 양성 (X)인데, 실제로 암을 가진 (A) 확률을 위 식을 이용해서 구하면 7.8%가 됩니다.
- Pr(X|A) - 암을 가진 경우 (A), 양성인 (X) 경우 이는 참 긍정으로 80%가 됩니다.
- Pr(A) - 암이 있는 경우 1%
- Pr(not A) - 암이 없는 경우 99%
- Pr(X | not A) - 암이 없는데 (~A) 양성 (X)이 된 경우, 이는 거짓 긍정 false positive으로 9.6%가 됩니다.
베이즈 필터 알고리즘
기본 필터 알고리즘은 $x_t$에 대해 다음과 같이 정리 할 수 있습니다.
예측 단계
첫 번째 단계는 베이즈 정리 bayes theorem을 이용하여 사전확률을 구하는데 이를 예측 과정 prediction step이라 합니다. 신뢰도 $\bar{bel}(x_t)$는 시간 t일때 측정 $z_t$를 반영하기 이전으로 이 단계에서는 동작이 발생함에 따라 기존의 가우시안과 공분산이 더해져 기존 정보를 잃어버리게 됩니다. 이 방정식의 RHS(right hand side) 우항은 사전 확률 계산을 위해서 전체 확률 법칙 law of total probabily를 사용합니다.
갱신 단계
다음은 갱신 단계로 correction or update step 시간 t에서의 측정 $z_t$를 통합하여 로봇의 신뢰도를 계산하는데, 로봇이 어디에 있는지 센서 정보로 반영하며 구하며 여기서 가우시안 분포들을 곱하게 됩니다. 가우시안의 곱 연산 결과로 평균은 사이에 위치하고, 공분산은 작아지게 됩니다.
복도를 따라 이동하며 문을 감지하는 센서를 가진 로봇.
위 그림은 베이즈 필터로 로봇의 위치를 추정하는 예시로서 로봇은 복도가 어떻게 생겼는지는(지도) 알지만 자신의 위치를 모르는 상태 입니다.
1. 맨 처음, 로봇은 지도상에서 어디에 위치하는지 모르므로 지도 전체에 동일한 확률들을 배정합니다.
2. 첫 센서 결과로 문이 존재하는것을 알았습니다. 로봇은 지도가 어떻게 되어있는지 알고는 있으나 문이 3개가 있으므로 어디에 있는지는 아직 알 수 없습니다. 그래서 각각의 문 위치에다가 같은 확률들이 배정됩니다.
3. 로봇이 앞으로 전진하면서 예측과정이 발생하는데 일부 정보를 잃어버리므로 4번째 다이어그램 처럼 가우시안의 분산이 커지게 됩니다. 마지막으로 신뢰도 분포는 이전 단계서의 사후확률과 동작후 상태를 컨볼루션한 결과로 평균이 우측으로 이동하게 됩니다.
4. 다시 측정 결과를 반영을 하는데 문이 존재한다면, 세 문들 앞에서 모두 문이 존재할 확률들은 같으므 5번째 다이어그램에서 계산한 사후확률을 구할 수 있습니다. 이 사후확률은 이전 사후확률과 측정의 컨벌루션 결과이며 로봇이 2번째 문 근처에 있음을 알 수 있습니다. 이때 분산은 작아지며 확률은 커지게 됩니다.
5. 이러한 동작과 갱신 과정이 반복되면 로봇은 6번째 다이어그램처럼 주위 환경에 대해 자신의 위치를 추정할 수 있게 됩니다.
베이즈 필터와 칼만 필터 구조
칼만 필터의 기본 구조와 컨셉은 베이즈 필터와 동일합니다. 다만 차이점은 칼만 필터의 수학적인 표현법인데, 칼만 필터는 가우시안을 이용한 베이지안 필터가 아닙니다. 베이즈 필터를 칼만 필터에 적용하기 위해서 가우시안을 히스토그램이 아닌 방법으로 나타낼건데, 베이즈 필터의 기본 공식은 다음과 같습니다.
$\bar{x}$는 사전 확률
$\iota$는 사전 확률 $\bar{x}$가 주어질때 측정에 대한 우도
$f_x$($\cdot$)은 속도를 이용해서 다음 위치를 예측할때 사용하는 모델이 됩니다.
*는 컨볼루션을 의미합니다.
칼만 게인
위에서 x는 사후확률이며 $\iota$와 $\bar{x}$는 가우시안이 됩니다.
그러므로 이 사후확률의 평균은 다음과 같이 정리할 수 있습니다.
다음과 같은 형태로 사전확률과 측정치를 스캐일링을 할수 있는데
여기서 분모가 정규화 시키므로 가중치를 하나에 합치며 K = $W_1$로 정리하면 다음과 같습니다.
칼만 게인에서의 분산은 다음과 같습니다.
K는 칼만게인이라 하며 측정치 $\mu_z$와 예측 평균 $\bar{\mu}$ 사이 값을 얼마나 반영할지 스케일링하는데 사용됩니다.
로봇이 동작하는 환경은 확률론적이기 때문입니다. 로봇과 주위 환경은 (시간에 대한 함수인)결정론적인 모델이 될수 없는데, 현실 세계의 센서들은 에러를 포함하고 있어 그 센서에 대한 평균과 분산으로 나타냅니다. 그래서 앞으로 평균과 분산을 이용한 모델을 다루게 될 것 입니다.
확률 변수의 기대값이란 무엇인가? expectation of a random variable
기대값은 확률의 평균 뿐만이 아니라
연속 영역에서의 형태로도 나타낼 수 있습니다.
import numpy as np
import random
x=[3,1,2]
p=[0.1,0.3,0.4]
E_x=np.sum(np.multiply(x,p))
print(E_x)
멀티모달보다 유니모달로 신뢰도를 나타내는 경우 이점은 무엇인가?
What is the advantage of representing the belief as a unimodal as opposed to multimodal?
움직이는 자동차의 위치를 두 가지 확률로 나타내는건 햇갈리므로 효율적이지는 않습니다.
분산, 공분산, 상관계수
분산
분산은 데이터가 퍼진 정도를 의미하며, 평균은 데이터가 얼마나 많은지를 나타내지는 않습니다.
x=np.random.randn(10)
np.var(x)
공분산
다변수 분포 multivariate distribution인 경우에 사용되는데, 예를 들면 2차원 공간에서의 로봇은 위치를 나타내는데 x, y 값을 사용할겁니다. 이를 나타내기 위해서 평균이 x, y인 정규 분포가 사용됩니다.
다변수 분포에서 평균 $\mu$는 아래의 행렬로 나타낼수 있는데
이처럼 분산도 표현할 수 있습니다.
하지만 중요한 점은 모든 변수들은 그 값에 대해 분산을 가지고 있는 점인데, 각각의 확률 변수들이 어떻게 서로 다른 형태 존재하도록 가능해 집니다. 역시 이 두 데이터셋에는 그들이 얼마나 관련되어있는지를 의미하는 상관 계수와도 관련 있습니다.
예를 들자면 보통 키가 증가하면 몸무게도 증가하는데, 이러한 변수들은 서로 상관관계를 가진다고 할 수 있습니다. 이 값들은 한 변수가 커지면 다른것도 커지므로 양의 상관 관계가 가진것이 됩니다.
이제 다변수 정규분포의 공분산을 공분산 행렬 covariance matrix를 사용해서 다음과 같이 나타내겠습니다.
이 이론에 따르면, 독립 확률 변수 n개 샘플의 평균은 샘플 사이즈를 늘릴수록 정규 분포의 형태가 되는 경향이 있습니다.(일반적으로 n >= 30)
import matplotlib.pyplot as plt
import random
a=np.zeros((100,))
for i in range(100):
x=[random.uniform(1,10) for _ in range(1000)]
a[i]=np.sum(x,axis=0)/1000
plt.hist(a)
가우시안 분포 gaussian distribution
가우시안은 연속 확률 분포 중 하나로 평균 $\mu$와 분산 $\sigma^2$ 두개의 파라미터로 나타내며 정의는 아래와 같습니다.
이는 평균 $\mu$과 표준편차 $\sigma$의 함수로서 종 모양이 되는 정규 분포의 특성을 나타냅니다.
import matplotlib.mlab as mlab
import math
import scipy.stats
mu = 0
variance = 5
sigma = math.sqrt(variance)
x = np.linspace(mu - 5*sigma, mu + 5*sigma, 100)
plt.plot(x,scipy.stats.norm.pdf(x, mu, sigma))
plt.show()
가우시안의 성질 Gaussian Properties
곱 multiplication
베이즈 필터 bayes filter의 측정 갱신 measurement update에서 그 알고리즘은 사전 확률 P($X_t$)와 측정 확률 P($Z_t$ | $X_t$)를 다음의 사후 확률을 구하기 위해 곱하여야 합니다.
여기서 분자 P(Z | X), P(X)는 N($\bar{\mu}$, $\bar{\sigma}^{1}$), N($\bar{\mu}$, $\bar{\sigma}^{2}$)을 따르는 가우시안이 됩니다.
이번 예시는 이변수 가우시안 분포 bivariate gaussian distribution를 시각화하는 예제로 2차원 표면에서 나오는 결과를 3차원으로 사영한 결과를 보여줍니다. 타원에서 가장 깊은 곳은 가장 높은 부분으로 주어진 (x,y)의 최대 확률이 됩니다.
#Example from:
#https://scipython.com/blog/visualizing-the-bivariate-gaussian-distribution/
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
# Our 2-dimensional distribution will be over variables X and Y
N = 60
X = np.linspace(-3, 3, N)
Y = np.linspace(-3, 4, N)
X, Y = np.meshgrid(X, Y)
# Mean vector and covariance matrix
mu = np.array([0., 1.])
Sigma = np.array([[ 1. , -0.5], [-0.5, 1.5]])
# Pack X and Y into a single 3-dimensional array
pos = np.empty(X.shape + (2,))
pos[:, :, 0] = X
pos[:, :, 1] = Y
def multivariate_gaussian(pos, mu, Sigma):
"""Return the multivariate Gaussian distribution on array pos.
pos is an array constructed by packing the meshed arrays of variables
x_1, x_2, x_3, ..., x_k into its _last_ dimension.
"""
n = mu.shape[0]
Sigma_det = np.linalg.det(Sigma)
Sigma_inv = np.linalg.inv(Sigma)
N = np.sqrt((2*np.pi)**n * Sigma_det)
# This einsum call calculates (x-mu)T.Sigma-1.(x-mu) in a vectorized
# way across all the input variables.
fac = np.einsum('...k,kl,...l->...', pos-mu, Sigma_inv, pos-mu)
return np.exp(-fac / 2) / N
# The distribution on the variables X, Y packed into pos.
Z = multivariate_gaussian(pos, mu, Sigma)
# Create a surface plot and projected filled contour plot under it.
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot_surface(X, Y, Z, rstride=3, cstride=3, linewidth=1, antialiased=True,
cmap=cm.viridis)
cset = ax.contourf(X, Y, Z, zdir='z', offset=-0.15, cmap=cm.viridis)
# Adjust the limits, ticks and view angle
ax.set_zlim(-0.15,0.2)
ax.set_zticks(np.linspace(0,0.2,5))
ax.view_init(27, -21)
plt.show()
이번 장에서는 연속 폐루프를 이용해서 횡방향과 종방향 오토파일럿을 설계하였습니다. 횡방향 오토파일럿은 내부 루프로 롤 자세 유지 루프를 외부 루프로 방위각 유지 루프로 이루어집니다.
종방향 오토파일럿은 고도에 따라 더 복잡하게 구성되는데 피치 자세 유지 루프는 모든 영역에서 사용됩니다. 이륙 영역에서는 비행체는 풀 스로틀 상태가 되며 이륙 피치각을 고정하도록 제어하게 됩니다. 상승 영역에서는 비행체는 풀 스로틀로, 피치, 대기 속도 유지 오토 파일럿 루프로 대기속도를 제어합니다. 하강 영역에서는 상승 영역과 비슷한데 대신 비행체에는 최소 스로틀을 줍니다. 고도 유지 영역에서 고도는 피치를 이용한 고도 오토파일럿 루프로 제어되고, 대기속도는 스로틀을 이용한 대기속도 오토파일럿 루프로 제어됩니다.
- 피치 각 제한 $e_{\theta}^{max}$, 댐핑 계수 $\zeta_{\theta}$, $\zeta_{h}$, $\zeta_{V}$, $\zeta_{V2}$와 고유 주파수 $\omega_{n_{v}}$, 고도 루프와 피치 이용 대기속도 루프의 분할 주파수 $W_h$, $W_{V_2}$
계산 고유 주파수들
- 식 (6.21)로 피치 루프 $\omega_{n_{\theta}}$ 고유 주파수 계산. $\omega_{n_{h}}$ = $\omega_{n_{\theta}}$ / $W_h$를 사용하여 고도 루프, $\omega_{n_{V2}}$ = $\omega_{n_{\theta}}$ / $W_{V_2}$를 사용하여 피치 이용 대기속도 루프의 고유 주파수 계산.
% compute trim conditions using 'mavsim_chap5_trim.slx'
% nominal airspeed P.Va0 specified above with aircraft parameters
gamma = 0*pi/180; % desired flight path angle (radians)
R = 10000000000; % desired radius (m) - use (+) for right handed orbit,
% (-) for left handed orbit
Va = 25;
% set initial conditions
x0 = [];
% specify which states to hold equal to the initial conditions
ix = [];
% specify initial inputs
u0 = [...
0;... % 1 - delta_e
0;... % 2 - delta_a
0;... % 3 - delta_r
1;... % 4 - delta_t
];
% specify which inputs to hold constant
iu = [];
% define constant outputs
y0 = [...
Va;... % 1 - Va
0;... % 2 - alpha
0;... % 3 - beta
];
% specify which outputs to hold constant
iy = [1,3];
% define constant derivatives
dx0 =
if R~=Inf, dx0(9) = Va*cos(gamma)/R; end % 9 - psidot
% specify which derivaties to hold constant in trim algorithm
idx = [3; 4; 5; 6; 7; 8; 9; 10; 11; 12];
% compute trim conditions
[x_trim,u_trim,y_trim,dx_trim] = trim('mavsim_trim',x0,u0,y0,ix,iu,iy,dx0,idx);
% check to make sure that the linearization worked (should be small)
norm(dx_trim(3:end)-dx0(3:end))
% P.u_trim = u_trim;
% P.x_trim = x_trim;
% set initial conditions to trim conditions
% initial conditions
MAV.pn0 = 0; % initial North position
MAV.pe0 = 0; % initial East position
MAV.pd0 = -200; % initial Down position (negative altitude)
MAV.u0 = x_trim(4); % initial velocity along body x-axis
MAV.v0 = x_trim(5); % initial velocity along body y-axis
MAV.w0 = x_trim(6); % initial velocity along body z-axis
MAV.phi0 = x_trim(7); % initial roll angle
MAV.theta0 = x_trim(8); % initial pitch angle
MAV.psi0 = x_trim(9); % initial yaw angle
MAV.p0 = x_trim(10); % initial body frame roll rate
MAV.q0 = x_trim(11); % initial body frame pitch rate
MAV.r0 = x_trim(12); % initial body frame yaw rate
trim 시뮬링크 모델이랑 트림 계산 파일이 저렇게 주어졌다.
xdot은 다음과 같으므로
값들은 아래와 같이 대입하면 될거같다
구현 하긴했는데
compute_trim에서 자꾸 dx0이랑 시뮬링크 블록상 연속 상태 크기랑 틀렸다고 에러뜬다.
한참 삽질하다보니 mav_dynamics에서 오일러각대신 쿼터니언을 쓰다보니 상태가 13개가 쓰여서 그렇더라
다시 챕터 3의 mav_dynamics를 오일러각으로 바꿧다.
function [sys,x0,str,ts,simStateCompliance] = mav_dynamics(t,x,u,flag,MAV)
switch flag
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0
[sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(MAV);
%%%%%%%%%%%%%%%
% Derivatives %
%%%%%%%%%%%%%%%
case 1
sys=mdlDerivatives(t,x,u,MAV);
%%%%%%%%%%
% Update %
%%%%%%%%%%
case 2
sys=mdlUpdate(t,x,u);
%%%%%%%%%%%
% Outputs %
%%%%%%%%%%%
case 3
sys=mdlOutputs(t,x);
%%%%%%%%%%%%%%%%%%%%%%%
% GetTimeOfNextVarHit %
%%%%%%%%%%%%%%%%%%%%%%%
case 4
sys=mdlGetTimeOfNextVarHit(t,x,u);
%%%%%%%%%%%%%
% Terminate %
%%%%%%%%%%%%%
case 9
sys=mdlTerminate(t,x,u);
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
% end sfuntmpl
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(MAV)
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded. This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;
sizes.NumContStates = 12;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 12;
sizes.NumInputs = 6;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
%
% initialize the initial conditions
%
x0 = [...
MAV.pn0;...
MAV.pe0;...
MAV.pd0;...
MAV.u0;...
MAV.v0;...
MAV.w0;...
MAV.phi0;...
MAV.theta0;...
MAV.psi0;...
MAV.p0;...
MAV.q0;...
MAV.r0;...
];
%
% str is always an empty matrix
%
str = [];
%
% initialize the array of sample times
%
ts = [0 0];
% Specify the block simStateCompliance. The allowed values are:
% 'UnknownSimState', < The default setting; warn and assume DefaultSimState
% 'DefaultSimState', < Same sim state as a built-in block
% 'HasNoSimState', < No sim state
% 'DisallowSimState' < Error out when saving or restoring the model sim state
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes
%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function sys=mdlDerivatives(t,x,uu, MAV)
pn = x(1);
pe = x(2);
pd = x(3);
u = x(4);
v = x(5);
w = x(6);
phi = x(7);
theta= x(8);
psi = x(9);
p = x(10);
q = x(11);
r = x(12);
fx = uu(1);
fy = uu(2);
fz = uu(3);
ell = uu(4);
m = uu(5);
n = uu(6);
cphi = cos(phi);
ctheta = cos(theta);
cpsi = cos(psi);
sphi = sin(phi);
stheta = sin(theta);
spsi = sin(psi);
pndot = u*ctheta*cpsi -v*(sphi*stheta*cpsi - cphi*spsi) +w*(cphi*stheta*cpsi + sphi*spsi);
pedot = u*ctheta*spsi + v*(sphi*stheta*spsi + cphi*cpsi) + w*(cphi*stheta*spsi - sphi*cpsi);
pddot = -stheta*u + v*sphi*ctheta + w*cphi *ctheta;
udot = r*v - q*w + 1/MAV.mass*fx;
vdot = p*w - r*u + 1/MAV.mass*fy;
wdot = q*u - p*v + 1/MAV.mass*fz;
phidot = p + sphi*tan(theta)*q + r*cphi*tan(theta);
thetadot = q*cphi -sphi*r;
psidot = sphi/ctheta*q + cphi/ctheta*r;
pdot = MAV.Gamma1*p*q-MAV.Gamma2*q*r+MAV.Gamma3*ell+MAV.Gamma4*n;
qdot = MAV.Gamma5*p*r-MAV.Gamma6*(p^2-r^2) +1/MAV.Jy*m;
rdot = MAV.Gamma7*p*q - MAV.Gamma1*q*r + MAV.Gamma4*ell + MAV.Gamma8*n;
sys = [pndot; pedot; pddot; udot; vdot; wdot; phidot; thetadot; psidot; pdot; qdot; rdot];
% end mdlDerivatives
%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)
sys = [];
% end mdlUpdate
%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x)
y = [...
x(1);
x(2);
x(3);
x(4);
x(5);
x(6);
x(7);
x(8);
x(9);
x(10);
x(11);
x(12);
];
sys = y;
% end mdlOutputs
%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block. Note that the result is
% absolute time. Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sampleTime = 1; % Example, set the next hit to be one second later.
sys = t + sampleTime;
% end mdlGetTimeOfNextVarHit
%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)
sys = [];
% end mdlTerminate
다음 값이 주어질때
트림 값
아래의 트림 시뮬레이션 모델에서
트림 계산 입력이 아래와 같이 주어질때
gamma = 2*pi/180; % desired flight path angle (radians)
R = 5000; % desired radius (m) - use (+) for right handed orbit,
% (-) for left handed orbit
Va = 100;
시뮬링크는 트림 상태를 계산하는 루틴을 제공하는데, help trim 으로 사용방법을 확인할 수 있습니다. 5.3장에서 봤던것 처럼 $V_a^*$, $\gamma^*$, $R^*$가 주어지면, $x^*$, $u^*$를 찾는것이 목표가 됩니다. $\dot{x^*}$ = f($x^*$, $u^*$ 로 여기서 x, u는 식 (5.17)과 (5.18)로 정의 됩니다. $\dot{x^*}$는 식 (5.21)에서 주어지며 f(x, u)는 식 (5.1) - (5.12)로 정의됩니다.
트림 명령의 형식은
[x, u, y, dx] = trim('sys', x0, u0, y0, ix, iu, uy, dx0, idx);
여기서 x는 계산된 트림상태 $x^*$, u는 계산됨 트림 입력 $u^*$, y는 계산된 트림 출력 $y^*$, dx는 계산된 상태의 미분 $\dot{x^*}$, 시뮬링크 모델은 sys.mdl에 명시되는데 모델의 상태들은 서브시스템 내 상태로 되어있고, 입력과 출력은 시뮬링크에서 inports, outports로 정의됩니다.
그림 F.1은 비행체 트림 계산하는 시뮬링크 모델로 입력은 4개의 inports로 서보 명령인 delta_e, delta_a, delta_r, delta_t 가 있습니다. 이 블록의 상태들은 시뮬링크 상태로 우리는 $\zeta$ = ($p_n$, $p_e$, $p_d$, u, u, w, $\phi$, $\theta$, $\psi$, p, q, r)$^T$가 됩니다. 출력은 세개의 outports이며 대기속도 $V_a$, 받음각 $\alpha$, $\beta$이며 이 아웃풋을 트림 명령에다가 전달하여 $V_a$ = $V_a^*$가 유지되도록 하겠습니다.
러더도 사용가능하면 $\beta^*$=0으로 유지시키는 트림 명령을 주어 정상선회 하도록 할수 있씁니다. 러더가 없는 경우 0상태로 만들수 없습니다.
트림 계산에서 문제는 시스템의 비선형 방정식을 풀어야 하는데, 많은 경우 시뮬링크 트림 명령에는 초기화 추정 값으로 상태 x0, 입력 u0, 출력 y0, 상태 미분 dx0이 필요합니다. 우리가 일부 상태나 입력, 출력 또는 미분 상태같은 시작 상태를 알아서 초기 상태를 쓸수 있으면, 사용가능한 값들을 인덱스 벡터 ix, iu, iy, idx로 전달할 수 있습니다.
우리의 상태가 아래와 같다면
이렇게 작성할 수 있습니다.
비슷하게 초기 상태, 입력, 출력들을 다음처럼 정의할수 있습니다.
그림 F.1 트림과 선형 상태 공간 모델 계산을 위한 시뮬링크 다이어그램
F.2 트림 수치 계산 Numerical Computation of Trim
시뮬링크 말고 다른환경에서 시뮬레이션이 개발된다면, 트림 루틴을 따로 작성해야합니다. 이번 장에서는 어떻게 하는지 간단히 설명하고 $V_a^*$, $\gamma^*$, $R^*$ 파라미터들로 상승 선회 트림 제어에 대해 살펴보고, 트림 찾기 알고리즘에서 입력으로 사용하겠습니다. 입력 파라미터 $V_a^*$, $\gamma^*$, $R^*$와 $\alpha$, $\beta$, $\phi$로 트림 상태와 입력 구하는 계산과정을 살펴보겠습니다. 주어진 $V_a^*$, $\gamma^*$, $R^*$값으로 트림값 $\alpha^*$, $\beta^*$, $\phi^*$을 찾아낸다면 이제 트림 상태와 입력같을 구할수 있게됩니다.
첫 단계는 상태 변수들을 $V_a^*$, $\gamma^*$, $R^*$로, 입력을 $\alpha^*$, $\beta^*$, $\phi^*$로 나타내는데 $V_a^*$, $\gamma^*$, $R^*$는 사용자 지정 입력이 되어 알고리즘은 최적의 트림 상태를 구할수 있는 $\alpha^*$, $\beta^*$, $\phi^*$ 계산합니다. 이 값으로 트림 상태 $x^*$, $u^*$를 찾는데 사용됩니다.
동체 좌표계 속도 $u^*$, $v^*$, $w^*$
식 (2.7)에서 동체 좌표계상 속도들은 $V_a^*$, $\alpha^*$, $\beta^*$ 를 이용해서 나타낼 수있습니다.
피치각 $\theta^*$
비행경로각의 정의로 다음과 같이 정리할 수있습니다.
각 변화율 p, q, r
각 변화율은 식 (3.2)로 오일러각으로 나타낼수 있습니다.
엘리베이터 $\delta_e$
$p^*$, $q^*$, $r^*$, $\delta_e^*$에 대해서 식 (5.11)을 풀면
에일러론 $\delta_a$과 러더 $\delta_r$
에일러론과 러더 명령은 식 (5.10), 식(5.12)을 풀어서 구할 수 있습니다.
F.2.1 트림 알고리즘
모든 관심변수와 제어입력은 $V_a^*$, $\gamma^*$, $R^*$,$\alpha^*$, $\beta^*$, $\phi^*$입니다. 트림 알고리즘의 입력은 $V^*$, $\gamma^*$, $R^*$이며, $\alpha^*$, $\beta^*$, $\phi^*$값을 찾기위해 다음의 최적화 알고리즘을 사용합니다.
이는 다음장에서 설명할 경사 하강 알고리즘으로 수치적으로 구할 수있습니다. 트림 알고리즘은 알고리즘 13에서 정리하였습니다.
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(MAV)
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded. This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;
sizes.NumContStates = 13;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 12;
sizes.NumInputs = 6;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
%
% initialize the initial conditions
%
x0 = [...
MAV.pn0;...
MAV.pe0;...
MAV.pd0;...
MAV.u0;...
MAV.v0;...
MAV.w0;...
MAV.e0;...
MAV.e1;...
MAV.e2;...
MAV.e3;...
MAV.p0;...
MAV.q0;...
MAV.r0;...
];
%
% str is always an empty matrix
%
str = [];
%
% initialize the array of sample times
%
ts = [0 0];
% Specify the block simStateCompliance. The allowed values are:
% 'UnknownSimState', < The default setting; warn and assume DefaultSimState
% 'DefaultSimState', < Same sim state as a built-in block
% 'HasNoSimState', < No sim state
% 'DisallowSimState' < Error out when saving or restoring the model sim state
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes
mdlOutputs으로 draw aircraft에서 사용할 변수 12개를 지정해서 출력해주면 된다.
이번장은 매트랩/시뮬링크 환경에 익숙한 사람들을 대상으로 진행하겠습니다. 다세한 정보는 매트랩/시뮬링크 문서를 참조해주시기 바랍니다. 시뮬링크는 상미분 방정식 사이 연결을 푸는대 필수적인 도구입니다.
시뮬링크 각각의 블록들이 다음의 구조대로 되어있다고 가정해봅시다. 여기서 $x_c$ $\in$ $\mathbb{R}$은 초기 조건이 $x_{c0}$인 연속 상태이고, $x_d$ $\in$ $\mathbb{R}^{n_d}$는 초기 조건이 $x_{d0}$인 이산 상태, u $\in$ $\mathbb{R}^m$는 블록 입력, y $\in$ $\mathbb{R}^p$는 블록의 출력이며, t는 시뮬레이션 경과 시간을 의미합니다.
s 함수는 다양한 방법으로 정의 가능한 시뮬링크 도구로 여기서 2가지 방법인 레벨 1 m file s함수와 C파일 s 함수에 대해 간단하게 살펴보겠습니다. c 파일 s 함수는 c코드로 컴파일되서 m 파일 s함수 보다 빠르게 실해오딥니다.
D.1 예시 : 2차 미분 방정식
이 장에서는 표준 2차 전달함수로 시스템을 구현하는 과정을 살펴보겠는데 여기서 레벨 1 m 파일 s 함수와 c 파일 s 함수를 사용하겠습니다.
우선 식 (D.4)를 상태 공간상에 표현하겠습니다. 제어 표준 형태 control canonical form로 나타내면
D.1.1 레벨 1 m 파일 s 함수
식 d.5, d.6을 시스템으로 구현하는 m 파일 s 함수 코드는 아래와 같습니다.
function [sys,x0,str,ts] = second_order_m(t,x,u,flag, zeta,wn)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
% initialize block
case 1,
sys=mdlDerivatives(t,x,u,zeta,wn);
% define xdot = f(t,x,u)
case 3,
sys=mdlOutputs(t,x,u,wn);
% define xup = g(t,x,u)
otherwise,
sys = [];
end
%==========================================================%
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 2;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 1;
sizes.NumInputs = 1;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = [0; 0]; % define initial conditions
str = []; % str is always an empty matrix
% initialize the array of sample times
ts = [0 0]; % continuous sample time
%==========================================================%
function xdot=mdlDerivatives(t,x,u,zeta,wn)
xdot(1) = −2*zeta*wn*x(1) − wn^2*x(2) + u;
xdot(2) = x(1);
%==========================================================%
function y=mdlOutputs(t,x,u,wn)
y = wn^2*x(2);
여기서 첫 줄은 m 파일 함수를 정의하는 부분이 됩니다. 이 함수의 입력은 항상 경과 시간 t, 상태 x(연속 상태와 이산 상태를 연결한), 입력 u, 와 플래그는 기본적으로 있어야 하며, 사용자 정의 입력 파라미터를 추가할 수도 있습니다. 이번 경우에는 $\zeta$와 $\omega_n$을 사용하겠씁니다.
시뮬링크 엔진은 s 함수를 호출하고 t, x, u, flag를 파라미터로 전달시키는데 flag == 0인경우 시뮬링크 엔진은 sys 구조체를 반환합니다. 여기서 sys 구조체는 초기 상태 x0, 빈 문자열 str, 샘플 타임배열인 ts로 구서오딥니다.
flag == 1인 경우 시뮬링크 엔진은 f(t, x, u) 함수를 반환하고, flag == 2일때는 g(t, x, u), flag==3 일때는 h(t, x, u)를 반환합니다.
스위치 문은 flag를 이용해서 적절한 함수를 호출하게 되는데, 초기화 구문을 살펴보면 연속 상태의 수, 이산 상태의 수, 출력 수, 입력의 수를 정의 합니다. 그리고, 출력을 입력으로 피드백 시킬것인지의 여부를 지정할 수도 있습니다.
