例三如下：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%日 期： 2015.10.10
%程序功能： Constant Velocity Model Kalman Filter Simulation(連續速度模型卡爾曼濾波仿真)
%程序變量描述 ：x_hat(:,i+1) 系統狀態變量
% F 系統矩陣
% w 噪聲
% z(:,i+1) 觀測值
% H 觀測矩陣
% P(:,:,i+1) 協方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; close all; clc;
%% Initial condition(初始化條件)
ts = 1; % Sampling time(採樣時間)
t = [0:ts:100];
T = length(t);
%% Initial state(初始化狀態)
x = [0 40 0 20]'; %第一和第三個元素分別表示XY軸座標，第二和第四個元素分別表示XY軸速度
x_hat = [0 0 0 0]';
%% Process noise covariance(過程噪聲協方差)
q = 5
Q = q*eye(2);
%% Measurement noise covariance(測量噪聲協方差)
r = 5
R = r*eye(2);
%% Process and measurement noise(過程和測量噪聲)
w = sqrt(Q)*randn(2,T); % Process noise(過程噪聲)
v = sqrt(R)*randn(2,T); % Measurement noise(測量噪聲)
%% Estimate error covariance initialization(估計誤差協方差初始化)
p = 5;
P(:,:,1) = p*eye(4); %P(:,:,1)指取所有的行和列
%==========================================================================
%% Continuous-time state space model(連續時間狀態空間模型)
%{
x_dot(t) = Ax(t)+Bu(t)
z(t) = Cx(t)+Dn(t)
%}
A = [0 1 0 0;
0 0 0 0;
0 0 0 1;
0 0 0 0];
B = [0 0;
1 0;
0 0;
0 1];
C = [1 0 0 0;
0 0 1 0];
D = [1 0;
0 1];
%% Discrete-time state space model(離散時間狀態空間模型)
%{
x(k+1) = Fx(k)+Gw(k)
z(k) = Hx(k)+Iv(k)
%}
sysc = ss(A,B,C,D);
sysd = c2d(sysc, ts, 'zoh'); %c2d函數功能爲將傳遞函數離散化
[F G H I] = ssdata(sysd);
%% Practice state of target(實際目標狀態)
for i = 1:T-1
x(:,i+1) = F*x(:,i);
end
x = x+G*w; % State variable with noise(帶噪聲的狀態變量)
z = H*x+I*v; % Measurement value with noise(帶噪聲的測量值)
%==========================================================================
%%% Kalman Filter
for i = 1:T-1
%% Prediction phase(預測階段)
x_hat(:,i+1) = F*x_hat(:,i); %(1) State estimate predict(狀態估計預測)
P(:,:,i+1) = F*P(:,:,i)*F'+G*Q*G'; %(2)Tracking error covariance predict(跟蹤誤差協方差預測)
%% Kalman gain(卡爾曼增益) %(3)
K = P(:,:,i+1)*H'*inv(H*P(:,:,i+1)*H'+R);
%% Updata step(階段更新)
x_hat(:,i+1) = x_hat(:,i+1)+K*(z(:,i+1)-H*x_hat(:,i+1)); %(4)State estimate update(狀態估計階段)
P(:,:,i+1) = P(:,:,i+1)-K*H*P(:,:,i+1); %(5)Tracking error covariance update(跟蹤誤差協方差更新)
end
%==========================================================================
%% Estimate error(估計誤差)
x_error = x-x_hat;
%% Graph 1 practical and tracking position
figure(1)
plot(x(1,:),x(3,:),'r'); %x(1,:)表示取第一行，所有列元素
hold on;
plot(x_hat(1,:),x_hat(3,:),'g.');
title('2D Target Position(2D 目標 位置)')
legend('實際位置','追蹤位置')
xlabel('X axis [m]')
ylabel('Y axis [m]')
hold off;
%% Graph 2
figure(2)
plot(t,x(1,:)),grid on;
hold on;
plot(t,x_hat(1,:),'r'),grid on;
title('實際和跟蹤位置 on X axis')
legend('實際位置','跟蹤位置')
xlabel('時間 [sec]')
ylabel('位置 [m]')
hold off;
%% Graph 3
figure(3)
plot(t,x_error(1,:)),grid on;
title('位置誤差 on X axis')
xlabel('時間 [sec]')
ylabel('位置均方根誤差 [m]')
hold off;
%% Graph 4
figure(4)
plot(t,x(2,:)),grid on;
hold on;
plot(t,x_hat(2,:),'r'),grid on;
title('實際和跟蹤速度 on X axis')
legend('實際速度','跟蹤速度')
xlabel('時間 [sec]')
ylabel('速度 [m/sec]')
hold off;
%% Graph 5
figure(5)
plot(t,x(4,:)),grid on;
hold on;
plot(t,x_hat(4,:),'r'),grid on;
title('實際和跟蹤速度 on Y axis')
legend('實際速度','跟蹤速度')
xlabel('時間 [sec]')
ylabel('速度 [m/sec]')
hold off;
%% Graph 6
figure(6)
plot(t,x_error(2,:)),grid on;
title('速度誤差 on X axis')
xlabel('時間 [sec]')
ylabel('速度均方根誤差 [m/sec]')
hold off;
%==========================================================================
