最优估计作业4

1第五讲作业1.问题描述：考虑时不变系统：vkXHZXXkkkkkkkkk,11其中15.0125.0,1005.010125.05.01量测矩阵H=[100]，系统噪声方差阵Q=[100]，量测噪声方差阵R=[200]，初始状态估计=[10000],=diag{45,500,1}。要求编制仿真程序验证Kalman滤波，要求输出：1.以友好的人机交互方式，选择过程噪声为均匀分布或高斯分布2.允许滤波终止时间在20-100步之间任意设置3.分别输出状态各维的预测方差、估计方差曲线4.经过一次仿真，分别输出状态各维的预测误差、估计误差曲线5.经过100次仿真，统计出状态各维的预测与估计的样本方差曲线2.Kalman滤波器原理与算法描述2.1Kalman滤波器原理对于问题1描述的带有量测噪声以及状态噪声的离散时不变系统，设计滤波器满足估计误差根据正交原理，量测zk+1已知，xk+1在zk+1的正交投影等价于已知b时a的线性最小方差估计。也就是说，对于所求目标满足：量测估计方差和状态量测协方差分别为：2状态估计误差为：量测预测误差为:系统增益其中，状态预测的协方差矩阵为：由于状态噪声和观测噪声协方差矩阵已知，状态转移矩阵已知，因此系统增益矩阵可以进行离线计算。2.2Kalman滤波算法流程与算法描述为了提高Kalman滤波器的速度，我们采用了离线计算系统增益Kk的方法，这样使得在实时计算滤波过程中的计算负担很小。离线部分算法如下表所示：Algorithm1Kalman滤波增益Kk的离线递推计算跟据上面的离线计算，我们可以得到每一步的滤波增益矩阵Kk，这样就可以快速的实现Kalman滤波的实时计算。在线部分计算如下：Algorithm2离散系统Kalman滤波估计3这样，就实时计算除了K时刻的状态估计值。3．人机交互界面根据题目1要求，使用MatlabGUI工具箱，设计人机交互界面，其中使用单选框组“UniformlyNoise”和“GaussianNoise”选择符合均匀分布或者高斯分布的噪声，使用文本框设置终止时间，按钮“1Simu”，“KalmanFilter”，“100Simu”分别控制进行1次仿真，对一次仿真状态进行Kalman滤波估计，进行100次仿真并进行Kalman滤波实验。右侧4图分别记录数据状态维度1，状态维度2，状态维度3和量测的实际值和估计值，其中横坐标表示时间，纵坐标表示数值，实线代表真实值，虚线代表估计值；左侧两图分别记录了1次仿真的预测误差、估计误差曲线和100次仿真的预测误差和估计误差方差的曲线。其中三条实线表示对三个维度的预测误差，三条虚线表示对三个维度的估计误差，总体界面如下图所示：图2:人机交互界面的实现高斯分布过程噪声下的实验结果4图3:高斯分布过程噪声下的实验结果结果分析：由上图可知，预测误差大于估计误差，可以看到，引入第k步观测值的信息，可以提高估计的精确程度。另一方面，随着时间的增大，系统的估计误差和预测误差收敛于一个稳定水平，但是精度不高。5均匀分布过程噪声下的实验结果图4:均匀分布过程噪声下的实验结果结果分析：由上图可知，当选均匀分布的过程噪声，滤波结果没有高斯分布的噪声，原因是：kalman滤波前提是噪声服从高斯分布。改变Q、R值对结果的影响改变过程噪声方差的大小、量测噪声方差的大小，当Q=10，R=20时，两个噪6声都降低为原来的1/10，结果如下：图5:改变Q、R值对结果的影响结果分析：降低噪声的大小，可以提高估计的精度，这是因为当噪声过大会淹没状态模型，造成估计精度不高4.Matlab源程序functionvarargout=kalman1(varargin)%KALMAN1M-fileforkalman1.fig%KALMAN1,byitself,createsanewKALMAN1orraisestheexisting%singleton*.%%H=KALMAN1returnsthehandletoanewKALMAN1orthehandleto%theexistingsingleton*.%%KALMAN1('CALLBACK',hObject,eventData,handles,...)callsthelocal%functionnamedCALLBACKinKALMAN1.Mwiththegiveninputarguments.%%KALMAN1('Property','Value',...)createsanewKALMAN1orraisesthe%existingsingleton*.Startingfromtheleft,propertyvaluepairsare%appliedtotheGUIbeforekalman1_OpeningFunctiongetscalled.An7%unrecognizedpropertynameorinvalidvaluemakespropertyapplication%stop.Allinputsarepassedtokalman1_OpeningFcnviavarargin.%%*SeeGUIOptionsonGUIDE'sToolsmenu.ChooseGUIallowsonlyone%instancetorun(singleton).%%Seealso:GUIDE,GUIDATA,GUIHANDLES%Edittheabovetexttomodifytheresponsetohelpkalman1%Begininitializationcode-DONOTEDITgui_Singleton=1;gui_State=struct('gui_Name',mfilename,...'gui_Singleton',gui_Singleton,...'gui_OpeningFcn',@kalman1_OpeningFcn,...'gui_OutputFcn',@kalman1_OutputFcn,...'gui_LayoutFcn',[],...'gui_Callback',[]);ifnargin&&ischar(varargin{1})gui_State.gui_Callback=str2func(varargin{1});endifnargout[varargout{1:nargout}]=gui_mainfcn(gui_State,varargin{:});elsegui_mainfcn(gui_State,varargin{:});end%Endinitializationcode-DONOTEDIT%---Executesjustbeforekalman1ismadevisible.functionkalman1_OpeningFcn(hObject,eventdata,handles,varargin)set(handles.noise_popupmenu,'Value',1);set(handles.step_edit,'String','80');set(handles.time_edit,'String','100');8set(handles.process_edit,'String','100');set(handles.measure_edit,'String','200');%Thisfunctionhasnooutputargs,seeOutputFcn.%hObjecthandletofigure%eventdatareserved-tobedefinedinafutureversionofMATLAB%handlesstructurewithhandlesanduserdata(seeGUIDATA)%varargincommandlineargumentstokalman1(seeVARARGIN)%Choosedefaultcommandlineoutputforkalman1handles.output=hObject;%Updatehandlesstructureguidata(hObject,handles);%UIWAITmakeskalman1waitforuserresponse(seeUIRESUME)%uiwait(handles.figure1);%---Outputsfromthisfunctionarereturnedtothecommandline.functionvarargout=kalman1_OutputFcn(hObject,eventdata,handles)%varargoutcellarrayforreturningoutputargs(seeVARARGOUT);%hObjecthandletofigure%eventdatareserved-tobedefinedinafutureversionofMATLAB%handlesstructurewithhandlesanduserdata(seeGUIDATA)%Getdefaultcommandlineoutputfromhandlesstructurevarargout{1}=handles.output;%---Executesonslidermovement.functionslider1_Callback(hObject,eventdata,handles)%hObjecthandletoslider1(seeGCBO)%eventdatareserved-tobedefinedinafutureversionofMATLAB%handlesstructurewithhandlesanduserdata(seeGUIDATA)%Hints:get(hObject,'Value')returnspositionofslider%get(hObject,'Min')andget(hObject,'Max')todeterminerangeofslider%---Executesduringobjectcreation,aftersettingallproperties.functionslider1_CreateFcn(hObject,eventdata,handles)9%hObjecthandletoslider1(seeGCBO)%eventdatareserved-tobedefinedinafutureversionofMATLAB%handlesempty-handlesnotcreateduntilafterallCreateFcnscalled%Hint:slidercontrolsusuallyhavealightgraybackground.ifisequal(get(hObject,'BackgroundColor'),get(0,'defaultUicontrolBackgroundColor'))set(hObject,'BackgroundColor',[.9.9.9]);end%---Executesonselectionchangeinnoise_popupmenu.functionnoise_popupmenu_Callback(hObject,eventdata,handles)guidata(hObject,handles);%hObjecthandletonoise_popupmenu(seeGCBO)%eventdatareserved-tobedefinedinafutureversionofMATLAB%handlesstructurewithhandlesanduserdata(seeGUIDATA)%Hints:contents=get(hObject,'String')returnsnoise_popupmenucontentsascellarray%con