%%?Kalman?filter?%%
%實現基本卡爾曼濾波，固定點N=45s卡爾曼平滑，滯后3s卡爾曼平滑
clc
close?all
clear?all

%%?條件初始化?%%
Phi=[1??0.5??0.25;
?????0???1????0.5;
?????0???0?????1?];
Tao=[0.125??0.5??1]‘;
H=[1??0??0];
Q=10;
R=200;
X_estimation（:1）=[100??0??0]‘;
X_real=zeros（3100）;
X_real（:1）=[120??20??0]‘;
P_estimation（::1）=diag（[450?500?1]）;

Step=50;
Num=100;

%%??生成仿真數據?%%
X_real（:1）=[120??20??0]‘;
for?i?=?2?:?size（X_real2）
??X_real（:i）?=Phi?*?X_real（:i-1）;
end
%?過程噪聲
%?str?=?input（‘產生過程噪聲輸入u表示均勻分布n表示高斯白噪聲:‘‘s‘）;
%?while?str~=‘u‘?&&?str~=‘n‘
%???str?=?input（‘產生過程噪聲輸入u表示均勻分布n表示高斯白噪聲:‘‘s‘）;
%?end
str?=?‘n‘;
Np?=?ProcessNoise（X_real0Qstr）;%產生過程噪聲
X_real?=?X_real?+?Tao*Np;
y?=?H?*?X_real;
Nm?=?MeasurementNoise（y0R）;%產生量測噪聲
y?=?y?+?Nm;%?含有高斯噪聲的量測數據

%%?基本卡爾曼濾波?%%
for?k=1:Step-1
????X_prediction（:k+1）=Phi*X_estimation（:k）;??%一步預測方程
????P_prediction（::k+1）=Phi*P_estimation（::k）*Phi‘+Tao*Q*Tao‘;??%一步預測協方差方程
????K（:k+1）=P_prediction（::k+1）*H‘*inv（H*P_prediction（::k+1）*H‘+R）;
????X_estimation（:k+1）=X_prediction（:k+1）+K（:k+1）*（y（k+1）-H*?X_prediction（:k+1））;%一步估計濾波方程
????P_estimation（::k+1）=P_prediction（::k+1）-P_prediction（::k+1）*H‘*inv（H*P_prediction（::k+1）*H‘+R）*（P_prediction（::k+1）*H‘）‘;%一步估計濾波協方差陣方程
end
%作圖
figure;
subplot（311）;
plot（X_real（11:46）‘r‘）;
hold?on;
plot（X_estimation（11:46）‘b-*‘）;
xlabel（‘Time?step/s‘）;
ylabel（‘Pos/m‘）;
legend（‘Real?Value‘‘Filter‘）;
subplot（312）;
plot（X_real（21:46）‘r‘）;
hold?on;
plot（X_estimation（21:46）‘b-*‘）;
xlabel（‘Time?step/s‘）;
ylabel（‘Vel/（m/s）‘）;
legend（‘Real?Value‘‘Filter‘）;
subplot（313）;
plot（X_real（21:46）‘r‘）;
hold?on;
plot（X_estimation（21:46）‘b-*‘）;
xlabel（‘Time?step/s‘）;
ylabel（‘Acc/（m/s^2）‘）;
legend（‘Real?Value‘‘Filter‘）;

%%?N1=45固定點平滑濾波X（N1lj）=X（45l50）
%條件初始化
N1=45;
X_smooth（:1）=X_estimation（:45）;%初始值X（NlN）=X（45l45）
W（::1）=P_estimation（::45）;%初始值W（NN）=P（NlN）=P（45l45）
P_smooth（::1）=P_estimation（::45）;%初始值P_smooth（NlN）=P（NlN）=P（45l45）
%X_smooth（Nlj）j=N+1初值X_smooth（45l45）X_smooth（Nlj）=X_smooth（45l46）
for?j=N1+1:Step
???W（::j-N1+1）=W（::j-N1）*Phi‘*（eye（3）-H‘*inv（R）*H*P_estimation（::j））;%固定點平滑增益矩陣
???X_smooth（:j-N1+1）=X_smooth（:j-N1）+W（::j-N1+1）*H‘*inv（R）*（y（j）-H*?X_prediction（:j））;%固定點平滑方程
???P_smooth（::j-N1+1）=P_smooth（::j-N1）-W（::j-N1+1）*（H‘*inv（R）*H*?P_prediction（::j）*H‘*inv（R）*H+H‘*inv（R）*H）*W（::j-N1+1）‘;%固定點平滑誤差方陣
end

e=X_estimation（:45）
s=X_smooth（:end）
t?=?X_real（:45）

%%?滯后3秒濾波X（klk+3）?%%?
%---------N2=3固定區間濾波---------------%
N2=3;
X_secsmooth（:N2+1）=X_estimation（: