目标定位基于matlab GUI卡尔曼滤波定位含Matlab源码 2309期

Posted 2023-01-14 海神之光

⛄一、基于粒子滤波污染源定位简介

1 目标定位原理
1.1 几何原理
理论上如图1所示，利用导引头测量两个不同位置的方位角信息及位置信息可确定两条直线，若观测目标静止，则两条直线交点即观测目标的位置。利用搭载导引头的侦测平台提供的位置、速度及姿态信息，可确定两个观测时刻的直线位置及方向，利用几何原理可定位待侦查目标的空间位置。

图1 无源定位系统几何原理(静止目标)
对于运动目标的无源定位如图2所示，不同时刻的定位线交叉后定位点会偏离目标真实位置，计算得到的目标运动轨迹也不是真实的目标运动轨迹。此时，根据几何原理，距离目标越远、侦测平台相对目标运动速度越快所计算的目标运动轨迹越准确，这就需要利用侦测平台的快速机动性，连续快速测量，增加对目标的测试频率，形成目标运动状态的估计。

图2 无源定位系统几何原理(运动目标)

1.2 数学模型
1)系统状态方程
无源定位系统依据侦测平台的位置和姿态信息计算目标的位置信息，可以以二维平面为例，计算无源定位的数学模型。设观测目标相对侦测平台的位置为x、y,速度为vx、vy,加速度为ax、ay,ΔT为系统数据传输周期，则系统的微分方程和离散状态方程分别为：

式(2)以矩阵形式可表示为：

X(k+1)=F(k)X(k)+G(k)u(k)+v(k) (3)

式中：X(k)为位置和速度组成的观测矩阵，X(k)=[x,y,vx,vy]T;u(k)为加速度矩阵，u(k)=[ax,ay]T,v(k)为状态误差；F(k),G(k)分别为状态转移矩阵和控制矩阵：

2)系统测量方程
红外导引头作为单站无源定位观测平台的单站定位系统，采用方位角作为观测量，则观测方程可表示为：

Z(k)=h(x(k),y(k))+w(k) (6)

其中：Z(k)为观测量矩阵；h(x(k),y(k) )为非线性观测函数；w(k)为观测噪声。
2 基于EKF的目标定位算法
扩展卡尔曼滤波是在卡尔曼提出的解决非线性问题的思想基础上发展而来，该方法的基本原理是由泰勒级数展开项的前几级部分来局部线性化，同时结合经典卡尔曼滤波算法完成计算仿真[11,12,13,14,15]。

EKF算法的递推过程如图1所示。

图3 EKF算法递推过程

⛄二、部分源代码

function varargout = shichajiaodu(varargin)
% SHICHAJIAODU M-file for shichajiaodu.fig
% SHICHAJIAODU, by itself, creates a new SHICHAJIAODU or raises the existing
% singleton*.
%
% H = SHICHAJIAODU returns the handle to a new SHICHAJIAODU or the handle to
% the existing singleton*.
%
% SHICHAJIAODU(‘CALLBACK’,hObject,eventData,handles,…) calls the local
% function named CALLBACK in SHICHAJIAODU.M with the given input arguments.
%
% SHICHAJIAODU(‘Property’,‘Value’,…) creates a new SHICHAJIAODU or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before shichajiaodu_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to shichajiaodu_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 modify the response to help shichajiaodu

% Last Modified by GUIDE v2.5 13-Apr-2019 02:09:35

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct(‘gui_Name’, mfilename, …
‘gui_Singleton’, gui_Singleton, …
‘gui_OpeningFcn’, @shichajiaodu_OpeningFcn, …
‘gui_OutputFcn’, @shichajiaodu_OutputFcn, …
‘gui_LayoutFcn’, [] , …
‘gui_Callback’, []);
if nargin && ischar(varargin1)
gui_State.gui_Callback = str2func(varargin1);
end

if nargout
[varargout1:nargout] = gui_mainfcn(gui_State, varargin:);
else
gui_mainfcn(gui_State, varargin:);
end
% End initialization code - DO NOT EDIT

% — Executes just before shichajiaodu is made visible.
function shichajiaodu_OpeningFcn(hObject, eventdata, handles, varargin)
% 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 shichajiaodu (see VARARGIN)

% Choose default command line output for shichajiaodu
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes shichajiaodu wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% — Outputs from this function are returned to the command line.
function varargout = shichajiaodu_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 from handles structure
varargout1 = handles.output;

function edit1_Callback(hObject, eventdata, handles)
% hObject handle to edit1 (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 edit1 as text
% str2double(get(hObject,‘String’)) returns contents of edit1 as a
% double

% — Executes during object creation, after setting all properties.
function edit1_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit1 (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(get(hObject,‘BackgroundColor’), get(0,‘defaultUicontrolBackgroundColor’))
set(hObject,‘BackgroundColor’,‘white’);
end

function edit2_Callback(hObject, eventdata, handles)
% hObject handle to edit2 (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 edit2 as text
% str2double(get(hObject,‘String’)) returns contents of edit2 as a double

% — Executes during object creation, after setting all properties.
function edit2_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit2 (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(get(hObject,‘BackgroundColor’), get(0,‘defaultUicontrolBackgroundColor’))
set(hObject,‘BackgroundColor’,‘white’);
end

function edit3_Callback(hObject, eventdata, handles)
% hObject handle to edit3 (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 edit3 as text
% str2double(get(hObject,‘String’)) returns contents of edit3 as a double

% — Executes during object creation, after setting all properties.
function edit3_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit3 (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(get(hObject,‘BackgroundColor’), get(0,‘defaultUicontrolBackgroundColor’))
set(hObject,‘BackgroundColor’,‘white’);
end

function edit4_Callback(hObject, eventdata, handles)
% hObject handle to edit4 (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 edit4 as text
% str2double(get(hObject,‘String’)) returns contents of edit4 as a double

% — Executes during object creation, after setting all properties.
function edit4_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit4 (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(get(hObject,‘BackgroundColor’), get(0,‘defaultUicontrolBackgroundColor’))
set(hObject,‘BackgroundColor’,‘white’);
end

function edit5_Callback(hObject, eventdata, handles)
% hObject handle to edit5 (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 edit5 as text
% str2double(get(hObject,‘String’)) returns contents of edit5 as a double

% — Executes during object creation, after setting all properties.
function edit5_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit5 (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(get(hObject,‘BackgroundColor’), get(0,‘defaultUicontrolBackgroundColor’))
set(hObject,‘BackgroundColor’,‘white’);
end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]李永生,董光焰,陈凯,谢亚峰,唐金力.基于卡尔曼滤波的无源定位精度分析[J].弹箭与制导学报. 2022,42(04)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除