例三如下：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%日 期： 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;
%==========================================================================
