선형칼만필터 매트랩 코드
in Kalman on Kalman
지금까지의 내용을 다 이해못해도 괜찮다.
간단한 매트랩 코드를 첨부한다.
function [X,P]=kalman_realtime(X,P,Sdata,dt,Q,R);
%%% Initialization %%%
F = [1 dt;0 1]; % State estimate matrix
H = [1 0]; % Observation matrix
%%% Prediction stage%%%
XPred = F*X;
PPred = F*P*F' + Q;
%%% Update stage %%%
z = Sdata; % An actual measurement
y = z - H*XPred ; % Comparion between measurement and predicted value
S = H*PPred*H' + R; % Covariance matrix Predict
K = PPred*H'/S; % Kalman gain update
XEst = XPred + K*y; % Final estimation
PEst = (eye(size(X,1)) - K*H)*PPred; % Covariance matrix update
%%% Update result %%%
X = XEst;
P = PEst;
end
c에 익숙하신 분들을 위해서 c 처럼 매트랩 코드를 변형 시켜보았다.
function [X,P]=kalman_c_realtime(X,P,Sdata,dt,Q,R);
F(1:4) = 0;
F(1) = 1.0;
F(2) = 0.0;
F(3) = dt;
F(4) = 1.0;
H(1:2) = 0;
H(1) = 1.0;
H(2) = 0.0;
for i = 1:2
XPred(i) = 0.0;
XPred(i) = XPred(i) + F(i) * X(1) + F(i + 2) * X(2);
b_F(i) = F(i) * P(1) + F(i + 2) * P(2);
b_F(i + 2) = F(i) * P(3) + F(i + 2) * P(4);
PPred(i) = (b_F(i) * F(1) + b_F(i + 2) * F(2)) + Q(i);
PPred(i + 2) = (b_F(i) * F(2) + b_F(i + 2) * F(4)) + Q(i + 2);
end
y = Sdata -(H(1) * XPred(1) + H(2) * XPred(2));
s = PPred(1) + R;
K(1) = (PPred(1))/s;
X(1) = XPred(1) + K(1) * y;
K(2) = (PPred(2))/s;
X(2) = XPred(2) + K(2) * y;
F(1) = 1.0;
F(2) = 0.0;
F(3) = 0.0;
F(4) = 1.0;
for i = 1:2
b_F(i) = F(i) - K(i);
b_F(i + 2) = F(i + 2);
P(i) = 0.0;
P(i + 2) = 0.0;
P(i) = P(i) + b_F(i) * PPred(1);
P(i) = P(i) + b_F(i + 2) * PPred(2);
P(i * 2) = P(i) + b_F(i) * PPred(3);
P(i + 2) = P(i) + b_F(i + 2) * PPred(4);
end
end
아니 왜
H행렬이 [1 0] 임?
왜냐하면 대부분의 경우에는, 거리와 속도를 동시에 독립적으로 계측할수있는 환경이 아니기 때문이다.
제어나 다른분야는 제쳐두고, 일반적인 SLAM 이나 사람존재검출에 이용되는 센서들은 복수의 검출원리를 퓨전으로 하지않는 이상은, 즉, 단일 센서군을 이용하는 한, 실제로 계측하는것은 거리뿐일것이다.
사실, H 를 단위행렬로 잡아도 상관은 없지만, 밑에서 설명할 공분산 행렬을 디테일하게 설정할 필요가 있어서, 경험상으로는 실제로 계측가능한 것들만 관측행렬에 집어넣는것을 추천한다.
선형칼만 필터 C 코드
void kalman_vel_realtime(double X[2], double P[4], double Sdata, double dt)
{
double F[4];
double H[2];
double Q[4];
double R;
double K[2];
double XPred[2];
double PPred[4];
double y;
double s;
int i;
double b;
double b_F[4];
/* %% Initialization %%% */
/* State estimate matrix */
F[0] = 1.0;
F[2] = dt;
F[1] = 0.0;
F[3] = 1.0;
/* Observation matrix */
H[0] = 1.0;
H[1] = 0.0;
/* Process noise covariance */
Q[0] = 0.0001;
Q[1] = 0.0;
Q[2] = 0.0;
Q[3] = 0.01;
/* Measurement noise covariance */
R = 0.0009;
/* %% Prediction stage%%% */
for (i = 0; i < 2; i++) {
XPred[i] = 0.0;
XPred[i] += F[i] * X[0] + F[i + 2] * X[1];
b_F[i] = F[i] * P[0] + F[i + 2] * P[1];
b_F[i + 2] = F[i] * P[2] + F[i + 2] * P[3];
PPred[i] = (b_F[i] * F[0] + b_F[i + 2] * F[2]) + Q[i];
PPred[i + 2] = (b_F[i] * F[1] + b_F[i + 2] * F[3]) + Q[i + 2];
}
/* %% Update stage %%% */
/* Comparion between measurement and predicted value */
y = Sdata - (H[0] * XPred[0] + H[1] * XPred[1]);
s = PPred[0] + R;
/* Covariance matrix Predict */
/* Kalman gain update */
K[0] = (PPred[0])/s;
X[0] = XPred[0] + K[0] * y;
K[1] = (PPred[1])/s;
X[1] = XPred[1] + K[1] * y;
/* Final estimation */
F[0] = 1.0;
F[1] = 0.0;
F[2] = 0.0;
F[3] = 1.0;
/* Covariance matrix update */
for (i = 0; i < 2; i++) {
b_F[i] = F[i] - K[i];
b_F[i + 2] = F[i + 2];
P[i] = 0.0;
P[i + 2] = 0.0;
P[i] += b_F[i] * PPred[0];
P[i] += b_F[i + 2]; * PPred[1];
P[i + 2] += b_F[i] * PPred[2];
P[i + 2] += b_F[i + 2] * PPred[3];
}
}
/* End of code (kalman_vel_realtime.c) */
c 라이브러리 안쓰냐?
마이콘에 들어갈 코드이기 때문에, 대부분의 C 라이브러리가 존재하지 않는다..
일반적으로 코딩을 할때는 매트랩 코드를 참고하여 라이브러리로 콤팩트하게 만들어보도록 하자.
R 과 Q 를 결정하는 방법
R 은 환경만 뒷받침 된다면 쉽다.
먼저 자신이 쓸려는 센서보다 정밀도가 높은 계측 장치를 준비해야한다.
예를들면 리니어 슬라이더의 엔코더라던지.
이렇게되면, 리니어 슬라이더로 타겟을 계속 움직이면서, 그 위치의 오차를 계속 구해간다.
오차의 통계치가 어느정도 모이면, 그 오차의 표준편차를 가지고 R 의 대각성분을 대입하면 된다.
R =
\begin{bmatrix}
\hat{\sigma}_r^{2} & 0\\
0 & \hat{\sigma}_v^{2}
\end{bmatrix}그럼 Q 는요
이것도 우리가 정확하게 모델 프로세싱으로인한 오차의 통계를 구할 수 있으면 좋겠지만 현실을 그리 녹록치 않다.
그래서 많은 경우, 근거가 빈약하긴 하지만, 다수의 논문들에서는 Q 또한 재귀적으로 구하는 방법을 연구하거나, 추측을 통해서 정수화 시키는 경우가 많은것 같다.
필자또한, Q 의 값을 여러가지 바꾸어 보면서 Q의 최적치를 튜닝하고있다.
그때에 이것만 알아두자, Q의 값을 크게 한다는것은, 모델의 오차가 클것이라고 값을 주는것
그리고 Q 값을 작게 한다는것은, 모델의 오차가 크지 않기때문에, 칼만게인이 모델쪽으로 쏠리게 되면서, 결과적으로 계측치에 대한 가중치가 줄어든다는 소리가 된다.
우리는 센서보다 정밀도가 높은 계측장치도없고 슬라이더도 없어요
그럼 걍 R 도 어림잡아 값을 넣어주자.
여기서부터는 개인의 감의 영역인것 같다.
끝