EEPW论坛

 %% 初始化数据
clc;clear;   % 清屏，清除之前变量
t=0:0.1:(20-0.1); % 时长
T=0.1;     % 采样时间
v=0.5;     % 初始速率

%% 设置化输入信号
y=ones(1,200);  %定义一零矩阵
y(1)=0;    %从0开始 
for n=2:200;    %计算位移曲线
    y(n)=y(n-1)+v*T;
end
a=randn(1,200) %设置噪声
s=y+a;  %信号加噪声

%% 已知参数设置

% x(k) = A*x(k-1) + B*u(k-1) + G*w(k-1) 系统方程
% y(k) = H*x(k) + v(k) 测量方程
A=[1 T
   0 1]; % 转移矩阵
% B=[1/2*(T)^2 T]'; %控制量为加速度或速度
H=[1 0]; %距离偏差 ……不肯定参数确定方法
 G=[1/2*(T)^2 T]';  %满足(1)  CV模型误差矩阵

Q=cov(randn(1,200)); % w(k)的协方差 ……实际中确定方法待定
R=2;% R=cov(randn(1,200)); % v(k)的协方差 ……实际中确定方法待定

% 误差信号协方差定义：P(k)=E[e(k)*e(k)']
% 两个信号的协方差阵：P(k)=E[  E[e1(k)^2, E[e1(k)*e2(k)]
%                             E[e2(k)*e1(k)], E[e2(k)^2 ]
% 初始值非零即可
P0=[5 6
    7 8];
P=[P0 zeros(2,2*199)];

% Y=[yp(n);yv(n)]，yp(n)滤波后位移，yv(n)滤波后速度
Y=zeros(2,200);
Y0=[0;1];
Y(:,1)=Y0; %初始
 %% 集中式卡尔曼基本方程
for n=1:200;
    i=(n-1)*2+1;
    K=P(:,i:i+1)*H'*inv(H*P(:,i:i+1)*H'+R);%卡尔曼增益 利用上一时刻预测的P值计算卡尔曼增益 (4)
    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));     %状态估计 利用卡尔曼增益与测量值修正预测值 (3)
    Y(:,n+1)=A*Y(:,n);                     %状态预测 预测下一状态值 (1)
    P(:,i:i+1)=(eye(2,2)-K*H)*P(:,i:i+1);  %估计方差阵 计算当前状态的P值 (5)
    P(:,i+2:i+3)=A*P(:,i:i+1)*A'+G*Q*G';   %进一步预测方差阵 利用当前状态P值预测下一状态P值(2)
end