这个卡尔曼滤波程序哪位大哥可以帮我解释一下?clearN=200;w(1)=0;w=randn(1,N);x(1)=0;a=1;for k=2:Nx(k)=a*x(k-1)+w(k-1); endV=randn(1,N);q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2;Y=c*x+V;p(1)=0;s(1)=0;for
来源:学生作业帮助网 编辑:作业帮 时间:2024/08/29 12:10:43
这个卡尔曼滤波程序哪位大哥可以帮我解释一下?clearN=200;w(1)=0;w=randn(1,N);x(1)=0;a=1;for k=2:Nx(k)=a*x(k-1)+w(k-1); endV=randn(1,N);q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2;Y=c*x+V;p(1)=0;s(1)=0;for
这个卡尔曼滤波程序哪位大哥可以帮我解释一下?
clear
N=200;
w(1)=0;
w=randn(1,N);
x(1)=0;
a=1;
for k=2:N
x(k)=a*x(k-1)+w(k-1);
end
V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;
p(1)=0;
s(1)=0;
for t=2:N
p1(t)=a.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');
这个卡尔曼滤波程序哪位大哥可以帮我解释一下?clearN=200;w(1)=0;w=randn(1,N);x(1)=0;a=1;for k=2:Nx(k)=a*x(k-1)+w(k-1); endV=randn(1,N);q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2;Y=c*x+V;p(1)=0;s(1)=0;for
clear
N=200;%取200个数
w(1)=0;
w=randn(1,N);%产生一个1×N的行向量,第一个数为0,w为过程噪声(其和后边的v在卡尔曼理论里均为高斯白噪声)
x(1)=0;%状态x初始值
a=1;%a为状态转移阵,此程序简单起见取1
for k=2:N
x(k)=a*x(k-1)+w(k-1); %系统状态方程,k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声(此处忽略了系统的控制量)
end
V=randn(1,N);%测量噪声
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2; %此方程未用到Rxx
q3=std(w);
Rww=q3.^2; %Rvv、Rww分别为过程噪声和测量噪声的协方差(此方程只取一组数方差与协方差相同)
c=0.2;
Y=c*x+V;%量测方差,c为量测矩阵,同a简化取为一个数
p(1)=0;%初始最优化估计协方差
s(1)=0;%s(1)表示为初始最优化估计
for t=2:N
p1(t)=a.^2*p(t-1)+Rww;%p1为一步估计的协方差,此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%b为卡尔曼增益,其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));%Y(t)-a*c*s(t-1)称之为新息,是观测值与一步估计得到的观测值之差,此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)
p(t)=p1(t)-c*b(t)*p1(t);%此式由一步估计的协方差得到此时刻最优化估计的协方差
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');%作图,红色为卡尔曼滤波,绿色为量测,蓝色为状态
%整体来说,此卡尔曼程序就是一个循环迭代的过程,给出初始的状态x和协方差p,得到下一时刻的x和p,循环带入可得到一系列的最优的状态估计值,此方法通常用于目标跟踪和定位.
%本人研究方向与此有关,有兴趣可以交流下.