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