卡尔曼平滑

%% 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（: