clc
clear?all;
close?all;

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

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

P?=?zeros（n）;?%估計值和真實值的誤差協方差矩陣
Qn?=?zeros（1N）;?%系統噪聲導致的小車位置偏差累積
Qn（1）?=?q*randn;
for?i?=?2:1:N
????Qn（i）?=?Qn（i-1）+q*randn;?%每一個dt內位置噪聲滿足高斯分布
end

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

xhat?=?zeros（sz）;?%估計值
xhatminus?=?zeros（sz）;?%預測值
Pminus?=?zeros（n）;?%預測值與真實值的誤差協方差矩陣
K?=?zeros（n（1）m（1））;?%卡爾曼增益矩陣
I?=?eye（n）;

for?k?=?9:N?%這里我們從時刻9處才開始計算，即小車已經運動了一段時間
????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‘）;