%卡尔曼滤波
clear
clc
N=200;
q=1;
w=CreateGauss(0,q,1,N) %系统预测的随机白噪声
Q=cov(w);%协方差
T=1;%采样间隔
A=[1,T;0,1];%状态转移矩阵
B=[(T^2)/2;T];%过程噪声分布矩阵
H=[1,0];%测量值的测量矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X1=zeros(2,200);%200个滤波值的数组
X0=[9;11];
X1(:,1)=X0;%初始化状态
X=zeros(2,200);%预测值矩阵
X(:,1)=X0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Z0=ones(1,200);%测量值矩阵
Z0(1)=100;
for n=2:200;
Z0(n)=Z0(n-1);%真实的轨迹
end
r=4;%测量值噪声的方差
V=CreateGauss(0,r,1,N); %测量值的随机白噪声
Z=Z0+V;%添加白噪声
R=cov(V);%协方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P=[q,q/T;q/T,2*q/(T^2)];%预测值的协方差
P1=[r,r/T;r/T,2*r/(T^2)];%滤波值的协方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N;
X(:,k)=A*X1(:,k-1);%预测值X(k|k-1)=A*X(k-1|k-1)
P=A*P1*A' + B*Q*B';%预测值的协方差P(K|k-1)=A*P(k-1|k-1)A'+Q
Kg=P*H'/(H*P*H'+R);%Kg(k)=p(k|k-1)*H'/(H*P(k|k-1)H'+R)
X1(:,k)=X(:,k)+Kg*(Z(:,k)-H*X(:,k));%Y滤波值X(k|k)=X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))%滤波值
P1=(eye(2,2)-Kg*H)*P;%滤波值的cov,P(k|k)=(E-Kg*H)*P(k|k-1)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=0:1:200-1;
x1=X1(1,:);
x=X(1,:);
plot(t,Z0,'-r');%真实轨迹
hold all;
plot(t,Z,'-.g');%带噪声的测量轨迹
hold all;
plot(t,x,'-.m');%预测轨迹
hold all;
plot(t,x1,'-b');%滤波轨迹
hold all;
legend('真实轨迹','带噪声的轨迹','预测轨迹','滤波轨迹','location','northwest');
axis([0 200 0 200]);
xlabel('时间');
ylabel('位置');
title('kalman滤波');
grid on
- 1
- 2
- 3
- 4
- 5
- 6
前往页