[matlab] kalman-filter

2020. 11. 22. 18:19

0. kalman-filter

- kalman filter 설명

http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

https://www-jlc.kek.jp/subg/offl/kaltest/doc/ReferenceManual.pdf

- Linear kalman filter

- the extended Kalman filter

1. 풀이

kalman filter를 구현하는 과제를 하게 되었다.

총 2가지 방법을 적용해보았다.

1) linear filter

2) Extend filter

2가지 방법 모두 필터가 매 시간마다 관측한 값을 통해 다음 값을 예측하는 부분이 핵심적인 부분이다.

1번 방법은 F, H, L, M 행렬이 동일한 값이라 가정하는 방법이다.

2번 방법은 F, H, L, M이 매번 변화하는 방법이다.

하지만, 이번 풀이에서는 F는 미리 정해진 행렬을 적용하였고, L과 M의 역할은 무시하였다.

H 만 계산하여 적용해주었다.

이때, 각 행렬은 jacobian matrix로

이와 같은 방법을 적용해주는 것이다.

즉, 다음과 같이 적용하면 Kalman filter를 제작할 수 있다.

2번 문제에서는 yt를 편미분하여 구하게 된 것이다.

이때, jacobian을 구하는 matlab의 함수가 존재하지만, 그렇게 하기 위해서는 syms로 변수를 만들어야한다.

그래서 손으로 계산해서 H에 대입해주었다.

2. 소스코드

- Linear kalman filter

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
%
% Kalman Filter Example
%
% Estimation of random constant
%
%
%
clear;
% initial parameters
n_iter = 50;
data_size = n_iter;
x = -0.37727;                        % truth value
sigma =0.1 ;                         % sigma
z = x+randn(1, data_size)*sigma;     % observations (normal about x, sigma= )
Q = 1e-5;                            % process variance
% allocate space for array
xhat = zeros(1, data_size);          % a posteri extimate of x
P = zeros(1, data_size);             % a posteri error estimate
xhatminus = zeros(1, data_size);     % a priori estimate of x
Pminus = zeros(1, data_size);        % a priori error estimate
K = zeros(1, data_size);             % gain or blending factor
R = 0.01;                            % Estimate of measurement variance
Xupdate = zeros(1, data_size);
Pupdate = zeros(1, data_size);
% initial guess
xhat(1) = 0;
P(1) = 1;
F = 1;                         % State estimate matrix
H = 1;                              % Observation matrix
%
%
for k=2:n_iter
    %%----------- Prediction stage ----------
    xhatminus(k) = F*xhat(k-1);                            
    Pminus(k) = F*P(k-1)*F' + Q;
    %%----------- update stage --------------
    K(k) = Pminus(k)*H'/(H*Pminus(k)*H' + R);              % Kalman gain update
    xhat(k) = xhatminus(k) + K(k)*(z(k)-H*xhatminus(k));   % Final estimation
    P(k) = (1 - K(k)*H)*Pminus(k);                    % Covariance matrix update
end
close all;
figure(1);
subplot(211);
hold on;
x1 = x*ones(1, data_size);
plot(x1, 'b');
plot(z, 'r-+');
plot(xhat, 'g');
legend('truth value', 'noisy measurements', 'a posteri estimate');
hold off;
title(['Simulation with R=' num2str(R)]);
xlabel('iteration');
ylabel('voltage');
subplot(212);
plot(Pminus(2:n_iter));
title('a priori error estimate');
xlabel('iteration');
ylabel('voltage^2');
%
Colored by Color Scripter
cs

- the extended Kalman filter

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
clear all
% Before filter execution
% System properties
%
T = 0.1; % Sampling time
N = 600; % Number of time steps for filter
N1 = 20; % Station 1 North coordinate
E1 = 0; % Station 1 East coordinate
N2 = 0; % Station 2 North coordinate
E2 = 20; % Station 2 East coordinate
%
% Step 1: Define noise assumptions
Q = diag([0 0 4 4]);
R = diag([1 1]);
%
% Step 2: Initialize state and covariance
x = zeros(4, N); % Initialize size of state estimate for all k
x(:,1) = [0; 0; 50; 50]; % Set initial state estimate
P0 = eye(4,4); % Set initial error covariance
%
% Simulation Only: Calculate true state trajectory for comparison
% Also calculate measurement vector
w = sqrt(Q)*randn(4, N); % Generate random process noise (from assumed Q)
v = sqrt(R)*randn(2, N); % Generate random measurement noise (from assumed R)
xt = zeros(4, N); % Initialize size of true state for all k
xt(:,1) = [0; 0; 50; 50] + sqrt(P0)*randn(4,1); % Set true initial state
yt = zeros(2, N); % Initialize size of output vector for all k
for k = 2:N
    xt(:,k) = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]*xt(:,k-1) + w(:,k-1);
    yt(:,k) = [sqrt((xt(1,k)-N1)^2 + (xt(2,k)-E1)^2); ...
    sqrt((xt(1,k)-N2)^2 + (xt(2,k)-E2)^2)] + v(:,k);
end
%
% Initialize and run EKF for comparison
xe = zeros(4,N);
xe(:,1) = x(:,1);
P = P0;
F = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]; % Linear prediction
 
for k = 2:N
    %----------- Prediction stage ----------
    x_temp = F*xe(:,k-1);                            
    P = F*P*F' + Q;
    % H = jacobian(yt(:,k-1), xt(:,k-1));
    H = [(xt(1,k)-N1)/yt(1,k) (xt(2,k)-E1)/yt(1,k) 0 0; (xt(1,k)-N1)/yt(2,k) (xt(2,k)-E1)/yt(2,k) 0 0];
    %----------- update stage --------------
    K = P*H'/(H*P*H'+R);              % Kalman gain update
    xe(:,k) = x_temp + K*(yt(:,k)-H*x_temp);   % Final estimation
    P = (P0 - K*H)*P;                    % Covariance matrix update
end
%
% Display results
figure(1);
t = T*(1:N);
for i = 1:4
subplot(4,2,2*i-1); plot(t,xe(i,:),'g-.', t,xt(i,:),'r--', 'LineWidth', 2);
xlabel('Time (s)'); ylabel(['x_',num2str(i)]); grid on;
legend('Estimate','State');
subplot(4,2,2*i); plot(t,xe(i,:)-xt(i,:),'g-.', 'LineWidth', 2);
xlabel('Time (s)'); ylabel(['\Deltax_',num2str(i)]); grid on;
legend('Estimate - State');
end
%
Colored by Color Scripter
cs