clc
clear?all;
close?all;

%初始化參數(shù)
delta_t?=?0.1;?%采樣時(shí)間間隔
t?=?0:delta_t:10;
N?=?length（t）;
sz?=?[2N];
g?=?10;?%加速度大小，對應(yīng)卡爾曼濾波中的控制向量
q?=?10;?%系統(tǒng)位置噪聲方差
Q?=?[q?0;0?0];?%系統(tǒng)噪聲協(xié)方差矩陣，大小為2X2（系統(tǒng)狀態(tài)中只包含位置、速度兩個(gè)分量）
r?=?5;?%測量位置噪聲方差
R?=?[r];?%測量噪聲協(xié)方差，大小為1X1（只測量了位置）
A?=?[1?delta_t;?0?1];?%狀態(tài)轉(zhuǎn)換矩陣，大小為2X2
B?=?[1/2*delta_t^2;?delta_t];?%控制轉(zhuǎn)換矩陣，大小為2X1
H?=?[10];?%觀測轉(zhuǎn)換矩陣，大小為1X2

n?=?size（Q）;
m?=?size（R）;

P?=?zeros（n）;?%估計(jì)值和真實(shí)值的誤差協(xié)方差矩陣
Qn?=?zeros（1N）;?%系統(tǒng)噪聲導(dǎo)致的小車位置偏差累積
Qn（1）?=?q*randn;
for?i?=?2:1:N
????Qn（i）?=?Qn（i-1）+q*randn;?%每一個(gè)dt內(nèi)位置噪聲滿足高斯分布
end

x?=?1/2*g*t.^2?+?Qn（int32（t/delta_t+1））;?%真實(shí)值
z?=?x?+?r.*randn（1N）;?%加入觀測噪聲的觀測值

xhat?=?zeros（sz）;?%估計(jì)值
xhatminus?=?zeros（sz）;?%預(yù)測值
Pminus?=?zeros（n）;?%預(yù)測值與真實(shí)值的誤差協(xié)方差矩陣
K?=?zeros（n（1）m（1））;?%卡爾曼增益矩陣
I?=?eye（n）;

for?k?=?9:N?%這里我們從時(shí)刻9處才開始計(jì)算，即小車已經(jīng)運(yùn)動(dòng)了一段時(shí)間
????xhatminus（:k）?=?A*xhat（:k-1）?+?B*g;?A*xhat（:k-1）+B*g;
????Pminus?=?A*P*A‘+Q;
????K?=?Pminus*H‘/（H*Pminus*H‘+R）;
????xhat（:k）?=?xhatminus（:k）+K*（z（k）-H*xhatminus（:k））;
????P?=?（I-K*H）*Pminus;
end
?
figure（1）
plot（tz）
hold?on
plot（txhat（1:）‘r-‘）
plot（txhatminus（1:）‘k-‘）;
hold?on
plot（tx（1:）‘g-‘）
legend（‘z‘‘xhat‘‘xhatminus‘‘x‘）;
xlabel（‘Iteration‘）;