最优估计作业4

1、第五讲作业1. 问题描述：考虑时不变系统：其中量测矩阵H = 1 0 0，系统噪声方差阵Q = 100，量测噪声方差阵R = 200，初始状态估计= 100 0 0, = diag45, 500, 1。要求编制仿真程序验证Kalman滤波，要求输出：1. 以友好的人机交互方式，选择过程噪声为均匀分布或高斯分布2. 允许滤波终止时间在20-100步之间任意设置3. 分别输出状态各维的预测方差、估计方差曲线4. 经过一次仿真，分别输出状态各维的预测误差、估计误差曲线5. 经过100次仿真，统计出状态各维的预测与估计的样本方差曲线2. Kalman滤波器原理与算法描述2.1 Kalman滤波器原理对

2、于问题1描述的带有量测噪声以及状态噪声的离散时不变系统，设计滤波器满足估计误差根据正交原理，量测zk+1已知，xk+1在zk+1的正交投影等价于已知b时a的线性最小方差估计。也就是说，对于所求目标 满足：量测估计方差和状态量测协方差分别为：状态估计误差为：量测预测误差为:系统增益其中，状态预测的协方差矩阵为：由于状态噪声和观测噪声协方差矩阵已知，状态转移矩阵已知，因此系统增益矩阵可以进行离线计算。2.2 Kalman滤波算法流程与算法描述为了提高Kalman滤波器的速度，我们采用了离线计算系统增益Kk的方法，这样使得在实时计算滤波过程中的计算负担很小。离线部分算法如下表所示： Algorith

3、m 1 Kalman滤波增益Kk的离线递推计算跟据上面的离线计算，我们可以得到每一步的滤波增益矩阵Kk，这样就可以快速的实现Kalman滤波的实时计算。在线部分计算如下：Algorithm 2 离散系统Kalman滤波估计这样，就实时计算除了K时刻的状态估计值。3人机交互界面根据题目1要求，使用Matlab GUI工具箱，设计人机交互界面，其中使用单选框组“Uniformly Noise”和“Gaussian Noise”选择符合均匀分布或者高斯分布的噪声，使用文本框设置终止时间，按钮“1 Simu”，“Kalman Filter”， “100 Simu”分别控制进行1次仿真，对一次仿真状态进

4、行Kalman滤波估计，进行100次仿真并进行Kalman滤波实验。右侧4图分别记录数据状态维度1，状态维度2，状态维度3和量测的实际值和估计值，其中横坐标表示时间，纵坐标表示数值，实线代表真实值，虚线代表估计值；左侧两图分别记录了1次仿真的预测误差、估计误差曲线和100次仿真的预测误差和估计误差方差的曲线。其中三条实线表示对三个维度的预测误差，三条虚线表示对三个维度的估计误差，总体界面如下图所示：图 2: 人机交互界面的实现高斯分布过程噪声下的实验结果图3: 高斯分布过程噪声下的实验结果结果分析：由上图可知，预测误差大于估计误差，可以看到，引入第k步观测值的信息，可以提高估计的精确程度。另一

5、方面，随着时间的增大，系统的估计误差和预测误差收敛于一个稳定水平，但是精度不高。均匀分布过程噪声下的实验结果图 4: 均匀分布过程噪声下的实验结果结果分析：由上图可知，当选均匀分布的过程噪声，滤波结果没有高斯分布的噪声，原因是：kalman滤波前提是噪声服从高斯分布。改变Q、R值 对结果的影响改变过程噪声方差的大小、量测噪声方差的大小，当Q=10，R=20时，两个噪声都降低为原来的1/10，结果如下：图 5:改变Q、R值对结果的影响结果分析：降低噪声的大小，可以提高估计的精度，这是因为当噪声过大会淹没状态模型，造成估计精度不高4.Matlab源程序function varargout = ka

6、lman1(varargin)% KALMAN1 M-file for kalman1.fig% KALMAN1, by itself, creates a new KALMAN1 or raises the existing% singleton*.% H = KALMAN1 returns the handle to a new KALMAN1 or the handle to% the existing singleton*.% KALMAN1('CALLBACK',hObject,eventData,handles,.) calls the local% functio

7、n named CALLBACK in KALMAN1.M with the given input arguments.% KALMAN1('Property','Value',.) creates a new KALMAN1 or raises the% existing singleton*. Starting from the left, property value pairs are% applied to the GUI before kalman1_OpeningFunction gets called. An% unrecognized pro

8、perty name or invalid value makes property application% stop. All inputs are passed to kalman1_OpeningFcn via varargin.% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one% instance to run (singleton)".% See also: GUIDE, GUIDATA, GUIHANDLES% Edit the above text to modi

9、fy the response to help kalman1% Begin initialization code - DO NOT EDITgui_Singleton = 1;gui_State = struct('gui_Name', mfilename, .'gui_Singleton', gui_Singleton, .'gui_OpeningFcn', kalman1_OpeningFcn, .'gui_OutputFcn', kalman1_OutputFcn, .'gui_LayoutFcn', ,

10、 .'gui_Callback', );if nargin && ischar(varargin1)gui_State.gui_Callback = str2func(varargin1);endif nargoutvarargout1:nargout = gui_mainfcn(gui_State, varargin:);elsegui_mainfcn(gui_State, varargin:);end% End initialization code - DO NOT EDIT% - Executes just before kalman1 is made

11、visible.function kalman1_OpeningFcn(hObject, eventdata, handles, varargin)set(handles.noise_popupmenu, 'Value', 1);set(handles.step_edit,'String','80');set(handles.time_edit,'String','100');set(cess_edit,'String','100');set(handles.

12、measure_edit,'String','200');% This function has no output args, see OutputFcn.% hObject handle to figure% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% varargin command line arguments to kalman1 (see VAR

13、ARGIN)% Choose default command line output for kalman1handles.output = hObject;% Update handles structureguidata(hObject, handles);% UIWAIT makes kalman1 wait for user response (see UIRESUME)% uiwait(handles.figure1);% - Outputs from this function are returned to the command line.function varargout

14、= kalman1_OutputFcn(hObject, eventdata, handles)% varargout cell array for returning output args (see VARARGOUT);% hObject handle to figure% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% Get default command line output f

15、rom handles structurevarargout1 = handles.output;% - Executes on slider movement.function slider1_Callback(hObject, eventdata, handles)% hObject handle to slider1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)%

16、 Hints: get(hObject,'Value') returns position of slider% get(hObject,'Min') and get(hObject,'Max') to determine range of slider% - Executes during object creation, after setting all properties.function slider1_CreateFcn(hObject, eventdata, handles)% hObject handle to slider1

17、(see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called% Hint: slider controls usually have a light gray background.if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColo

18、r')set(hObject,'BackgroundColor',.9 .9 .9);end% - Executes on selection change in noise_popupmenu.function noise_popupmenu_Callback(hObject, eventdata, handles)guidata(hObject,handles);% hObject handle to noise_popupmenu (see GCBO)% eventdata reserved - to be defined in a future version

19、of MATLAB% handles structure with handles and user data (see GUIDATA)% Hints: contents = get(hObject,'String') returns noise_popupmenu contents as cell array% contentsget(hObject,'Value') returns selected item from noise_popupmenu% - Executes during object creation, after setting all

20、 properties.function noise_popupmenu_CreateFcn(hObject, eventdata, handles)% hObject handle to noise_popupmenu (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called% Hint: popupmenu controls usually have a

21、white background on Windows.% See ISPC and COMPUTER.If ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'set(hObject,'BackgroundColor','white');endfunction step_edit_Callback(hObject, eventdata, handles)guidata(hObject,hand

22、les);% hObject handle to step_edit (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of step_edit as text% str2double(get(hObject,'String') returns co

23、ntents of step_edit as a double% - Executes during object creation, after setting all properties.function step_edit_CreateFcn(hObject, eventdata, handles)% hObject handle to step_edit (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created un

24、til after all CreateFcns called% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'set(hObject,'BackgroundColor','white');endfunction

25、 time_edit_Callback(hObject, eventdata, handles)guidata(hObject,handles);% hObject handle to time_edit (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of ti

26、me_edit as text% str2double(get(hObject,'String') returns contents of time_edit as a double% - Executes during object creation, after setting all properties.function time_edit_CreateFcn(hObject, eventdata, handles)% hObject handle to time_edit (see GCBO)% eventdata reserved - to be defined i

27、n a future version of MATLAB% handles empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor

28、9;set(hObject,'BackgroundColor','white');endfunction process_edit_Callback(hObject, eventdata, handles)guidata(hObject,handles);% hObject handle to process_edit (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data

29、(see GUIDATA)% Hints: get(hObject,'String') returns contents of process_edit as text% str2double(get(hObject,'String') returns contents of process_edit as a double% - Executes during object creation, after setting all properties.function process_edit_CreateFcn(hObject, eventdata, han

30、dles)% hObject handle to process_edit (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called% Hint: edit controls usually have a white background on Windows.% See ISPC and COMPUTER.if ispc && isequal

31、(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'set(hObject,'BackgroundColor','white');endfunction measure_edit_Callback(hObject, eventdata, handles)guidata(hObject,handles);% hObject handle to measure_edit (see GCBO)% eventdata reserved - to b

32、e defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% Hints: get(hObject,'String') returns contents of measure_edit as text% str2double(get(hObject,'String') returns contents of measure_edit as a double% - Executes during object creation

33、, after setting all properties.function measure_edit_CreateFcn(hObject, eventdata, handles)% hObject handle to measure_edit (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called% Hint: edit controls usually

34、 have a white background on Windows.% See ISPC and COMPUTER.if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'set(hObject,'BackgroundColor','white');end% - Executes on button press in start_pushbutton.function start_push

35、button_Callback(hObject, eventdata, handles)% hObject handle to start_pushbutton (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)% handles.N=50;% handles.Q = 100;% handles.R = 200;handles.val=get(handles.noise_po

36、pupmenu,'Value');switch handles.valcase 2handles.noise=1;case 3handles.noise=0;endhandles.N=str2num(get(handles.step_edit,'String');handles.num=str2num(get(handles.time_edit,'String');handles.Q=str2num(get(cess_edit,'String');handles.R=str2num(get(handles.m

37、easure_edit,'String');handles.A = 1 0.5 0.1250 1 0.50 0 1;handles.B = 0.125 0.5 1'handles.H = 1 0 0;handles.x_ture(:,1) = 120 20 0'handles.z(:,1)=0;if handles.noise=1randn('state',sum(100*clock);for i=1:handles.Nhandles.v = randn(1,1)*sqrt(handles.Q);handles.x_ture(:,i+1) = h

38、andles.A*handles.x_ture(:,i)+handles.B*handles.v;endelsefor i=1:handles.Nhandles.v = rand(1,1)*sqrt(handles.Q);handles.x_ture(:,i+1) = handles.A*handles.x_ture(:,i)+handles.B*handles.v;endendfor n=1:handles.numfor i=1:handles.Nhandles.w = randn(1,1)*sqrt(handles.R);handles.z(:,i+1) = handles.H*handl

39、es.x_ture(:,i+1)+handles.w;endhandles.x_esti(:,1) = 100 0 0'handles.p_esti(:,:,1) = diag(45,500,1);%kalmanfor j=1:handles.Nhandles.x_fore(:,j) = handles.A*handles.x_esti(:,j);handles.p_fore(:,:,j) = handles.A*handles.p_esti(:,:,j)*handles.A'+handles.B*handles.Q*handles.B'handles.k=handle

40、s.p_fore(:,:,j)*handles.H'*inv(handles.H*legend(handles.axes1, 'True Value','Forest Value','Estimate Value','Location','Best');legend(handles.axes2, 'True Value','Forest Value','Estimate Value','Location','Best');leg

41、end(handles.axes3, 'True Value','Forest Value','Estimate Value','Location','Best');title(handles.axes1,'Dimension 1');title(handles.axes2,'Dimension 2');title(handles.axes3,'Dimension 3');plot(handles.axes4,1:handles.N,handles.P_fore(1,

42、1:handles.N),'b-.',1:handles.N,handles.P_esti(1,1:handles.N),'r-')plot(handles.axes5,1:handles.N,handles.P_fore(2,1:handles.N),'b-.',1:handles.N,handles.P_esti(2,1:handles.N),'r-')plot(handles.axes6,1:handles.N,handles.P_fore(3,1:handles.N),'b-.',1:handles.N,h

43、andles.P_esti(3,1:handles.N),'r-')legend(handles.axes4,'Forecasting Variance of Dim 1','Estimation Variance of Dim 1','Location','Best');% legend(handles.axes5,'Forecasting Variance of Dim 2','Estimation Variance of Dim 2','Location',&#

44、39;Best');% legend(handles.axes6,'Forecasting Variance of Dim 3','Estimation Variance of Dim 3','Location','Best');title(handles.axes4,'1 simulation:Variance of Dimension 1');title(handles.axes5,'1 simulation:Variance of Dimension 2');title(handles

45、.axes6,'1 simulation:Variance of Dimension 3');plot(handles.axes7,1:handles.N,handles.x_ture(1,2:handles.N+1)-handles.x_fore(1,1:handles.N),'g',1:handles.N,handles.x_ture(1,2:handles.N+1)-handles.x_esti(1,2:handles.N+1),'r-');plot(handles.axes8,1:handles.N,handles.x_ture(2,2:

46、handles.N+1)-handles.x_fore(2,1:handles.N),'g',1:handles.N,handles.x_ture(2,2:handles.N+1)-handles.x_esti(2,2:handles.N+1),'r-');plot(handles.axes9,1:handles.N,handles.x_ture(3,2:handles.N+1)-handles.x_fore(3,1:handles.N),'g',1:handles.N,handles.x_ture(3,2:handles.N+1)-handle

47、s.x_esti(3,2:handles.N+1),'r-');legend(handles.axes7,'Forecasting Error of Dim 1','Estimation Error of Dim 1','Location','Best');% legend(handles.axes8,'Forecasting Error of Dim 2','Estimation Error of Dim 2','Location','Best');% legend(handles.axes9,'Forecasting Error of Dim 3','Estimation Error of Dim 3','Location','Best');title(handles.axes7,'1 simulation:Error of Dimension 1&#