MATLAB Function ブロックを使用したレーダー追跡
この例では、MATLAB Function ブロックを使用して航空機の位置を推定するカルマン フィルターを作成する方法を示します。位置を推定した後、モデルは外部 MATLAB® 関数を呼び出して追跡データをプロットします。
モデルの検査
RadarTrackingExample
モデルを開きます。
パラメーターの設定と加速度データの初期化
物理システムを表すために、モデルはモデル ワークスペースで次のパラメーターを初期化します。
g
— 重力による加速度tauc
— 航空機の交差軸加速度の相関時間taut
— 航空機の推力軸加速度の相関時間speed
— 航空機の y 方向の初速度deltat
— レーダー更新頻度
XY Acceleration Model
サブシステムは加速度データをモデル化して出力します。Band-Limited White Noise ブロック INS Acceleration Data
は、モデルが "X-Y" 平面の直交座標で航空機の加速度データを決定するために使用するデータを生成します。
加速度の位置への変換
拡張カルマン フィルターは、極座標の位置データを使用します。航空機の位置を取得するために、Second-Order Integratorブロックは加速度データを 2 回積分します。この位置データは直交座標であるため、XY to Range Bearing
サブシステムは位置データを極座標に変換します。実際のレーダー データをより適切に表現するために、モデルは、Band-Limited White Noise ブロックを使用してノイズを生成し、Gain ブロックを使用してノイズ強度を調整することにより、位置データにノイズを追加します。最後に、モデルは Zero-Order Hold ブロック Sample and Hold
を使用して、固定時間間隔で連続時間データをサンプリングして保持してから、MATLAB Function ブロックの拡張カルマン フィルターに渡します。
拡張カルマン フィルターの表示
MATLAB Function ブロックを開いて、拡張カルマン フィルターを表示します。関数は 2 つの入力引数 measured
と deltat
を受け取ります。measured
は極座標での入力位置データ、deltat
はワークスペース変数の値です。MATLAB Function ブロック パラメーター変数の設定を参照してください。フィルターを実装するために、関数は 2 つの永続変数 P
と xhat
を定義し、タイム ステップ間に関数でこれらの変数を保存します。フィルターを実装した後、ブロックは次の 2 つの出力を生成します。
residual
— 残差を含むスカラーxhatout
— 直交座標での航空機の推定位置と速度を含むベクトル
function [residual, xhatOut] = extendedKalman(measured, deltat) % Radar Data Processing Tracker Using an Extended Kalman Filter
%% Initialization persistent P; persistent xhat if isempty(P) xhat = [0.001; 0.01; 0.001; 400]; P = zeros(4); end
%% Compute Phi, Q, and R
Phi = [1 deltat 0 0; 0 1 0 0 ; 0 0 1 deltat; 0 0 0 1];
Q = diag([0 .005 0 .005]);
R = diag([300^2 0.001^2]);
%% Propagate the covariance matrix and track estimate
P = Phi*P*Phi' + Q;
xhat = Phi*xhat;
%% Compute observation estimates:
Rangehat = sqrt(xhat(1)^2+xhat(3)^2);
Bearinghat = atan2(xhat(3),xhat(1));
% Compute observation vector y and linearized measurement matrix M
yhat = [Rangehat;
Bearinghat];
M = [ cos(Bearinghat) 0 sin(Bearinghat) 0
-sin(Bearinghat)/Rangehat 0 cos(Bearinghat)/Rangehat 0 ];
%% Compute residual (Estimation Error)
residual = measured - yhat;
% Compute Kalman Gain:
W = P*M'/(M*P*M'+ R);
% Update estimate
xhat = xhat + W*residual;
% Update Covariance Matrix
P = (eye(4)-W*M)*P*(eye(4)-W*M)' + W*R*W';
xhatOut = xhat;
モデルのシミュレーション
モデルをシミュレートして結果を表示します。モデルは推定位置と実際の位置をログに記録し、ベース ワークスペースに保存します。次に、モデルは "StopFcn" コールバックで補助関数 plotRadar
を呼び出すことにより、シミュレーションの終了時にこのデータを使用して結果をプロットします。プロットには、極座標での実際の軌跡と推定の軌跡、フィート単位での距離の推定残差、実際の位置、測定位置、および推定位置が表示されます。
補助関数
関数 plotRadar
は、MATLAB Function ブロックからのログ データ出力をプロットします。
function plotRadar(varargin) % Radar Data Processing Tracker plotting function
% Get radar measurement interval from model
deltat = 1;
% Get logged data from workspace
data = locGetData();
if isempty(data) return; % if there is no data, no point in plotting else XYCoords = data.XYCoords; measurementNoise = data.measurementNoise; polarCoords = data.polarCoords; residual = data.residual; xhat = data.xhat; end
% Plot data: set up figure if nargin > 0 figTag = varargin{1}; else figTag = 'no_arg'; end
figH = findobj('Type','figure','Tag', figTag);
if isempty(figH) figH = figure; set(figH,'WindowState','maximized','Tag',figTag); end
clf(figH)
% Polar plot of actual/estimated position figure(figH); % keep focus on figH axesH = subplot(2,3,1,polaraxes); polarplot(axesH,polarCoords(:,2) - measurementNoise(:,2), ... polarCoords(:,1) - measurementNoise(:,1),'r')
hold on rangehat = sqrt(xhat(:,1).^2+xhat(:,3).^2); bearinghat = atan2(xhat(:,3),xhat(:,1)); polarplot(bearinghat,rangehat,'g'); legend(axesH,'Actual','Estimated','Location','south');
% Range Estimate Error figure(figH); % keep focus on figH axesH = subplot(2,3,4); plot(axesH, residual(:,1)); grid; set(axesH,'xlim',[0 length(residual)]); xlabel(axesH, 'Number of Measurements'); ylabel(axesH, 'Range Estimate Error - Feet') title(axesH, 'Estimation Residual for Range')
% East-West position XYMeas = [polarCoords(:,1).*cos(polarCoords(:,2)), ... polarCoords(:,1).*sin(polarCoords(:,2))]; numTSteps = size(XYCoords,1); t_full = 0.1 * (0:numTSteps-1)'; t_hat = (0:deltat:t_full(end))';
figure(figH); % keep focus on figH axesH = subplot(2,3,2:3); plot(axesH, t_full,XYCoords(:,2),'r'); grid on;hold on plot(axesH, t_full,XYMeas(:,2),'g'); plot(axesH, t_hat,xhat(:,3),'b'); title(axesH, 'E-W Position'); legend(axesH, 'Actual','Measured','Estimated','Location','Northwest'); hold off
% North-South position figure(figH); % keep focus on figH axesH = subplot(2,3,5:6); plot(axesH, t_full,XYCoords(:,1),'r'); grid on;hold on plot(axesH, t_full,XYMeas(:,1),'g'); plot(axesH, t_hat,xhat(:,1),'b'); xlabel(axesH, 'Time (sec)'); title(axesH, 'N-S Position'); legend(axesH, 'Actual','Measured','Estimated','Location','Northwest'); hold off end
% Function "locGetData" logs data to workspace function data = locGetData % Get simulation result data from workspace
% If necessary, convert logged signal data to local variables if evalin('base','exist(''radarLogsOut'')') try logsOut = evalin('base','radarLogsOut'); if isa(logsOut, 'Simulink.SimulationData.Dataset') data.measurementNoise = logsOut.get('measurementNoise').Values.Data; data.XYCoords = logsOut.get('XYCoords').Values.Data; data.polarCoords = logsOut.get('polarCoords').Values.Data; data.residual = logsOut.get('residual').Values.Data; data.xhat = logsOut.get('xhat').Values.Data; else assert(isa(logsOut, 'Simulink.ModelDataLogs')); data.measurementNoise = logsOut.measurementNoise.Data; data.XYCoords = logsOut.XYCoords.Data; data.polarCoords = logsOut.polarCoords.Data; data.residual = logsOut.residual.Data; data.xhat = logsOut.xhat.Data; end catch %#ok<CTCH> data = []; end else if evalin('base','exist(''measurementNoise'')') data.measurementNoise = evalin('base','measurementNoise'); data.XYCoords = evalin('base','XYCoords'); data.polarCoords = evalin('base','polarCoords'); data.residual = evalin('base','residual'); data.xhat = evalin('base','xhat'); else data = []; % something didn't run, skip retrieval end end end
参考
MATLAB Function | Extended Kalman Filter (System Identification Toolbox)
関連するトピック
- Aerospace Blockset
- Create Aerospace Models (Aerospace Blockset)
- MATLAB Function ブロックを使用したらせん状の銀河形成シミュレーション