卡尔曼实验实验报告.docVIP

Kalman 滤波实验报告 一、实验内容简介 卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，根据系统的量测值来消除随机干扰， 卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统， 采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值它适合于实时处理和计算机运算。 二、自编实验 2.1自编仿真程序 %定义仿真的长度 nlen=20; %Define the system. a=1; h=3; %噪声协方差 Q=0.01; R=2; % 所有阵列的预分配内存 % 1) the state. % 2) the a priori error. 先验误差 初步估计 x=zeros(1,nlen); z=zeros(1,nlen); xapriori=zeros(1,nlen); xaposteriori=zeros(1,nlen); residual=zeros(1,nlen); papriori=ones(1,nlen); paposteriori=ones(1,nlen); k=zeros(1,nlen); % 计算过程和测量误差 w1=randn(1,nlen); v1=randn(1,nlen); %运行该脚本一次生成多个具有相同 %.噪声 w=w1*sqrt(Q); v=v1*sqrt(R); % 初始条件对状态、x x_0=1.0; %现状和后验概率方差的初始猜测 xaposteriori_0=1.5; paposteriori_0=1; %计算基于初始状态和后验概率方差的所有值的第一个估计,将计算在一个循环中的其余步骤。 %Calculate the state and the output计算状态和输出 x(1)=a*x_0+w(1); z(1)=h*x(1)+v(1);%预测方程 xapriori(1)=a*xaposteriori_0; residual(1)=z(1)-h*xapriori(1); papriori(1)=a*a*paposteriori_0+Q;%校正方程 k(1)=h*papriori(1)/(h*h*papriori(1)+R); paposteriori(1)=papriori(1)*(1-h*k(1)); xaposteriori(1)=xapriori(1)+k(1)*residual(1); %计算值的估计 for j=2:nlen, %计算状态和输出 x(j)=a*x(j-1)+w(j); z(j)=h*x(j)+v(j); %预测方程 xapriori(j)=a*xaposteriori(j-1); residual(j)=z(j)-h*xapriori(j); papriori(j)=a*a*paposteriori(j-1)+Q; %校正方程 k(j)=h*papriori(j)/(h*h*papriori(j)+R); paposteriori(j)=papriori(j)*(1-h*k(j)); xaposteriori(j)=xapriori(j)+k(j)*residual(j); end j=1:nlen; %状态以及状态估计 subplot(221); h1=stem(j+0.25,xapriori,b); hold on h2=stem(j+0.5,xaposteriori,g); h3=stem(j,x,r); hold off % 设计的格式 legend([h1(1) h2(1) h3(1)],a priori,a posteriori,exact); title(State with a priori and a posteriori elements); ylabel(State, x); xlim=[0 length(j)+1]; set(gca,XLim,xlim); % 协方差 subplot(222); h1=stem(j,papriori,b); hold on; h2=stem(j,paposteriori,g); hold off legend([h1(1) h2(1)],a priori,a posteriori); title