卡尔曼滤波器程序解析(2)

离散 卡尔曼滤波器程序解析(2)

例三如下：

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %日 期： 	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);