基于卡尔曼滤波的定位跟踪算法仿真

基于卡尔曼滤波的定位跟踪算法仿真

%---------------------------------------------%
%					      %
%          工作室提供代做matlab仿真	      %
%					      %
%  详情请访问：http://cn.mikecrm.com/5k6v1DP  %
%					      %
%---------------------------------------------%

function kalmanFiltering
%%
clc
close all

%%
Z=（1:2:200）; %观测值  汽车的位置  也就是我们要修改的量
noise=6*randn（1100）; %方差为1的高斯噪声
Z=Z+noise;
X=[0 ; 0 ]; %初始状态
P=[1 0;0 1]; %状态协方差矩阵
F=[1 1;0 1]; %状态转移矩阵
Q=[0.00010;0  0.0001]; %状态转移协方差矩阵
H=[10]; %观测矩阵
R=1; %观测噪声协方差矩阵
figure;
hold on;
for i = 1:100
%基于上一状态预测当前状态  
X_ = F*X;
% 更新协方差  Q系统过程的协方差  这两个公式是对系统的预测
P_ = F*P*F‘+Q;
% 计算卡尔曼增益
K = P_*H‘/（H*P_*H‘+R）;
% 得到当前状态的最优化估算值  增益乘以残差
X = X_+K*（Z（i）-H*X_）;
%更新K状态的协方差
P = （eye（2）-K*H）*P_;
scatter（X（1） X（2）4）; %画点，横轴表示位置，纵轴表示速度
end

end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        942  2018-09-19 07:59  基于卡尔曼滤波的定位跟踪算法仿真\a.m

     文件         55  2018-08-27 11:33  基于卡尔曼滤波的定位跟踪算法仿真\【源码使用必读】.url

     目录          0  2018-09-19 08:05  基于卡尔曼滤波的定位跟踪算法仿真

----------- ---------  ---------- -----  ----

                  997                    3