%---------------------------------------------%
%					??????%
%??????????工作室提供代做matlab仿真	??????%
%					??????%
%??詳情請訪問：http://cn.mikecrm.com/5k6v1DP??%
%					??????%
%---------------------------------------------%

%======卡爾曼算法，狀態參數有兩個，速度與位置，估計其值======
%=======對勻速運動物體的跟蹤：速度和位移================

%%
clc?;
clear;
n=100;??????????????????%100個點
Yk??????????=zeros（2n）;
Xk_real?????=zeros（2n）;
Xk_real（:1）?=[5015]‘;%position?and?speed
Z（:1）=50;%觀測值初始化
F=[11;01];%狀態轉移矩陣2*2
H=[1?0];%觀測矩陣1*2
R=1;
%?Q=1e-3;%經驗取值
Xk（:1）=[3;1];
P0=eye（2）;

%觀測值與狀態值生成
for?i=2:n
????Xk_real（:i）=F*Xk_real（:i-1）;%+sqrt（Q）*randn（21）;%2*2與2*1=2*1
????Yk（i）=H*Xk_real（:i）?+sqrt（R）*randn（1）;
end

%卡爾曼迭代
for?i=2:n
????Xn?????=F*Xk（:i-1）;?????????%2*1
????Pk?????=F*P0*F‘;???????????%2*2
????K??????=Pk*H‘*inv（R+H*Pk*H‘）;???%2*1
????Xk（:i）=Xn+K*（Yk（i）-H*Xn）;???????%2*1
????P0?????=（eye（2）-K*H）*Pk;
end

%速度誤差與位移誤差
error_p=Xk_real（1:）-Xk（1:）;
error_s=Xk_real（2:）-Xk（2:）;
%%
figure;
plot（Xk_real（1:）‘r-‘）;
hold?on
plot（Xk（1:）‘y*‘）plot（error_p）;
grid?on
xlabel（‘間隔‘）;
ylabel（‘位置‘）;
legend（‘真實位置軌跡‘‘濾波位置軌跡‘‘誤差‘）;
title（‘真實軌跡與濾波軌跡‘）;

figure（）;
plot（Xk_real（2:）‘y-‘）;
hold?on
plot（Xk（2:）‘g*-‘）plot（error_s‘r*-‘）;
grid?on
xlabel（‘間隔‘）;
ylabel（‘速度‘）;
legend（‘真實速度軌跡‘‘濾波速度軌跡‘‘誤差‘）;
title（‘真實軌跡與濾波軌跡‘）;

????

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????1522??2018-09-19?08:09??適合卡爾曼濾波的初學者，仿真了勻速運動狀態下，卡爾曼穩定跟蹤物體軌跡的代碼\a.m

?????文件?????????55??2018-08-27?11:33??適合卡爾曼濾波的初學者，仿真了勻速運動狀態下，卡爾曼穩定跟蹤物體軌跡的代碼\【源碼使用必讀】.url

?????目錄??????????0??2018-09-16?11:01??適合卡爾曼濾波的初學者，仿真了勻速運動狀態下，卡爾曼穩定跟蹤物體軌跡的代碼

-----------?---------??----------?-----??----

?????????????????1577????????????????????3