Введение

Control System Toolbox™ предлагает три команды для нелинейной оценки состояния:

Типичный рабочий процесс использования этих команд следующий:

Этот пример сначала использует unscentedKalmanFilter команда для демонстрации этого рабочего процесса. Затем он демонстрирует использование particleFilter.

Моделирование и дискретизация объекта

Сигма-точечный фильтр Калмана (UKF) требует функции, которая описывает эволюцию состояний от одного временного шага до следующего. Обычно это называется функцией перехода состояния. unscentedKalmanFilter поддерживает следующие две формы функций:

Здесь f (..) - переходная функция состояния, x - состояние, w - шум процесса. u является необязательным и представляет дополнительные входы f, для образца входов или параметров системы. u может быть задано как нуль или больше аргументов функции. Аддитивный шум означает, что состояние и технологический шум связаны линейно. Если отношение нелинейно, используйте вторую форму. Когда вы создаете unscentedKalmanFilter объект, вы задаете f (..), а также является ли технологический шум аддитивным или не аддитивным.

Система в этом примере является осциллятором Ван дер Поля с mu = 1. Эта система с 2 состояниями описывается следующим набором нелинейных обыкновенных дифференциальных уравнений (ОДУ):

Обозначите это уравнение как $$\underset{}{\overset{\dot{}}{x}}=f_c(x)$$, где $$x=[x_1;x_2]$$. Шум процесса w не появляется в системную модель. Можно предположить, что это добавка для простоты.

unscentedKalmanFilter требует функцию перехода в дискретном времени состояния, но модели объекта управления непрерывного времени. Можно использовать приближение в дискретном времени к модели в непрерывном времени. Дискретизация Эйлера является одним из распространенных методов приближения. Предположим, что ваш шаг расчета $$T_s$$, и обозначить динамику в непрерывном времени, которую вы имеете как $$\underset{}{\overset{\dot{}}{x}}=f_c(x)$$. Дискретизация Эйлера аппроксимирует оператор производной как $$\underset{}{\overset{\dot{}}{x}}\approx \frac{x[k+1]-x[k]}{T_s}$$. Получившаяся функция перехода состояния в дискретном времени является:

Точность этого приближения зависит от шага расчета $$T_s$$. Меньше $$T_s$$ значения обеспечивают лучшие приближения. Кроме того, можно использовать другой метод дискретизации. Например, семейство методов Рунге-Кутты более высокого порядка обеспечивает более высокую точность за счет больших вычислительных затрат, учитывая фиксированный шаг расчета $$T_s$$.

Создайте эту функцию перехода к состоянию и сохраните ее в файле с именем vdpStateFcn.m. Использование шага расчета $$T_s=0.05s$$. Вы предоставляете эту функцию unscentedKalmanFilter во время конструкции объекта.

addpath(fullfile(matlabroot,'examples','control','main')) % add example data
type vdpStateFcn

function x = vdpStateFcn(x) 
% vdpStateFcn Discrete-time approximation to van der Pol ODEs for mu = 1. 
% Sample time is 0.05s.
%
% Example state transition function for discrete-time nonlinear state
% estimators.
%
% xk1 = vdpStateFcn(xk)
%
% Inputs:
%    xk - States x[k]
%
% Outputs:
%    xk1 - Propagated states x[k+1]
%
% See also extendedKalmanFilter, unscentedKalmanFilter

%   Copyright 2016 The MathWorks, Inc.

%#codegen

% The tag %#codegen must be included if you wish to generate code with 
% MATLAB Coder.

% Euler integration of continuous-time dynamics x'=f(x) with sample time dt
dt = 0.05; % [s] Sample time
x = x + vdpStateFcnContinuous(x)*dt;
end

function dxdt = vdpStateFcnContinuous(x)
%vdpStateFcnContinuous Evaluate the van der Pol ODEs for mu = 1
dxdt = [x(2); (1-x(1)^2)*x(2)-x(1)];
end

Моделирование датчика

unscentedKalmanFilter также нужна функция, которая описывает, как состояния модели связаны с измерениями датчика. unscentedKalmanFilter поддерживает следующие две формы функций:

h (..) - функция измерения, v - шум измерения. u является необязательным и представляет дополнительные входы h, для образца входов или параметров системы. u может быть задано как нуль или больше аргументов функции. Можно добавить дополнительные системные входы, следующие за u термином. Эти входы могут отличаться от входов в функции перехода состояния.

В данном примере предположим, что у вас есть измерения первого состояния $$x_1$$ в пределах некоторой процентной ошибки:

Это попадает в категорию неаддитивного измерения шума, потому что измерение шум не просто добавляется к функции состояний. Вы хотите оценить оба $$x_1$$ и $$x_2$$ от шумных измерений.

Создайте эту функцию перехода и сохраните ее в файле с именем vdpMeasurementNonAdditiveNoiseFcn.m. Вы предоставляете эту функцию unscentedKalmanFilter во время конструкции объекта.

type vdpMeasurementNonAdditiveNoiseFcn

function yk = vdpMeasurementNonAdditiveNoiseFcn(xk,vk)
% vdpMeasurementNonAdditiveNoiseFcn Example measurement function for discrete
% time nonlinear state estimators with non-additive measurement noise.
%
% yk = vdpNonAdditiveMeasurementFcn(xk,vk)
%
% Inputs:
%    xk - x[k], states at time k
%    vk - v[k], measurement noise at time k
%
% Outputs:
%    yk - y[k], measurements at time k
%
% The measurement is the first state with multiplicative noise
%
% See also extendedKalmanFilter, unscentedKalmanFilter

%   Copyright 2016 The MathWorks, Inc.

%#codegen

% The tag %#codegen must be included if you wish to generate code with 
% MATLAB Coder.

yk = xk(1)*(1+vk);
end

Сигма-точечный фильтр Калмана

Создайте фильтр, предоставив указатели на функцию для переходов и функций измерения, затем угадайте ваше начальное состояние. Модель перехода состояния имеет аддитивный шум. Это настройка по умолчанию в фильтре, поэтому вам не нужно ее задавать. Модель измерения имеет неаддитивный шум, который вы должны задать путем установки HasAdditiveMeasurementNoise свойство объекта как false. Это должно быть сделано во время конструкции объекта. Если ваша заявка имеет неаддитивный технологический шум в функции перехода состояния, задайте HasAdditiveProcessNoise свойство как false.

% Your initial state guess at time k, utilizing measurements up to time k-1: xhat[k|k-1]
initialStateGuess = [2;0]; % xhat[k|k-1]
% Construct the filter
ukf = unscentedKalmanFilter(...
    @vdpStateFcn,... % State transition function
    @vdpMeasurementNonAdditiveNoiseFcn,... % Measurement function
    initialStateGuess,...
    'HasAdditiveMeasurementNoise',false);

Предоставьте свои знания о ковариации шума измерения

R = 0.2; % Variance of the measurement noise v[k]
ukf.MeasurementNoise = R;

The ProcessNoise свойство сохраняет ковариацию шума процесса. Он устанавливается для учета неточностей модели и эффекта неизвестных нарушений порядка на объекте. У нас есть истинная модель в этом примере, но дискретизация вводит ошибки. Этот пример не включал никаких нарушений порядка для простоты. Установите его в диагональную матрицу с меньшим шумом в первом состоянии и больше во втором состоянии, чтобы отразить знание о том, что второе состояние больше затронуто ошибками моделирования.

ukf.ProcessNoise = diag([0.02 0.1]);

Оценка с использованием предсказания и правильных команд

В вашем приложении данные измерений, поступающие с оборудования в режиме реального времени, обрабатываются фильтрами по мере их поступления. Эта операция показана в этом примере путем генерации сначала набора данных измерений и затем подачи его в фильтр по одному шагу за раз.

Симулируйте осциллятор Ван дер Поля в течение 5 секунд со шаг расчета фильтра 0,05 [с], чтобы сгенерировать истинные состояния системы.

T = 0.05; % [s] Filter sample time
timeVector = 0:T:5;
[~,xTrue]=ode45(@vdp1,timeVector,[2;0]);

Сгенерируйте измерения, принимая, что датчик измеряет первое состояние со стандартным отклонением 45% ошибки в каждом измерении.

rng(1); % Fix the random number generator for reproducible results
yTrue = xTrue(:,1);
yMeas = yTrue .* (1+sqrt(R)*randn(size(yTrue))); % sqrt(R): Standard deviation of noise

Предварительно выделите пространство для данных, которые вы будете анализировать позже

Nsteps = numel(yMeas); % Number of time steps
xCorrectedUKF = zeros(Nsteps,2); % Corrected state estimates
PCorrected = zeros(Nsteps,2,2); % Corrected state estimation error covariances
e = zeros(Nsteps,1); % Residuals (or innovations)

Выполните онлайн-оценку состояний x, используя correct и predict команды. Предоставляйте сгенерированные данные фильтру по одному временному шагу за раз.

for k=1:Nsteps
    % Let k denote the current time.
    %
    % Residuals (or innovations): Measured output - Predicted output
    e(k) = yMeas(k) - vdpMeasurementFcn(ukf.State); % ukf.State is x[k|k-1] at this point
    % Incorporate the measurements at time k into the state estimates by
    % using the "correct" command. This updates the State and StateCovariance
    % properties of the filter to contain x[k|k] and P[k|k]. These values
    % are also produced as the output of the "correct" command.
    [xCorrectedUKF(k,:), PCorrected(k,:,:)] = correct(ukf,yMeas(k));
    % Predict the states at next time step, k+1. This updates the State and
    % StateCovariance properties of the filter to contain x[k+1|k] and
    % P[k+1|k]. These will be utilized by the filter at the next time step.
    predict(ukf);
end

Сигма-точечные фильтры Калмана и валидация

Постройте график истинного и предполагаемого состояний с течением времени. Также постройте график измеренного значения первого состояния.

figure();
subplot(2,1,1);
plot(timeVector,xTrue(:,1),timeVector,xCorrectedUKF(:,1),timeVector,yMeas(:));
legend('True','UKF estimate','Measured')
ylim([-2.6 2.6]);
ylabel('x_1');
subplot(2,1,2);
plot(timeVector,xTrue(:,2),timeVector,xCorrectedUKF(:,2));
ylim([-3 1.5]);
xlabel('Time [s]');
ylabel('x_2');

Верхний график показывает истинное, оцененное и измеренное значение первого состояния. Фильтр использует системную модель и информацию о шуме ковариации, чтобы получить улучшенную оценку по сравнению с измерениями. Нижний график показывает второе состояние. Фильтр успешно дает хорошую оценку.

Валидация неразумной и расширенной эффективности фильтра Калмана обычно выполняется с помощью обширных симуляций Монте-Карло. Эти симуляции должны проверять изменения реализации шума процесса и измерения, работу объекта в различных условиях, начальное состояние и ковариационные догадки состояния. Ключевым сигналом интереса, используемым для проверки оценки состояния, являются невязки (или инновации). В этом примере вы выполняете остаточный анализ для одной симуляции. Постройте график невязок.

figure();
plot(timeVector, e);
xlabel('Time [s]');
ylabel('Residual (or innovation)');

Невязки должны иметь:

Среднее значение невязок:

mean(e)

ans = -0.0012

Это мало относительно величины невязок. Автокорреляция невязок может быть вычислена с помощью функции xcorr в Signal Processing Toolbox.

[xe,xeLags] = xcorr(e,'coeff'); % 'coeff': normalize by the value at zero lag
% Only plot non-negative lags
idx = xeLags>=0;
figure();
plot(xeLags(idx),xe(idx));
xlabel('Lags');
ylabel('Normalized correlation');
title('Autocorrelation of residuals (innovation)');

Корреляция является маленькой для всех лагов, кроме 0. Средняя корреляция близка к нулю, и корреляция не показывает каких-либо значительных неслучайных изменений. Эти характеристики повышают доверие в эффективности фильтра.

На самом деле истинные состояния никогда не доступны. Однако при выполнении симуляций вы имеете доступ к реальным состояниям и можете посмотреть на ошибки между предполагаемым и истинным состояниями. Эти ошибки должны удовлетворять критериям, подобным остаточным:

Во-первых, посмотрите на ошибку и $$1\sigma$$ ограничения неопределенности из ковариационной оценки ошибки фильтра.

eStates = xTrue-xCorrectedUKF;
figure();
subplot(2,1,1);
plot(timeVector,eStates(:,1),...               % Error for the first state
    timeVector, sqrt(PCorrected(:,1,1)),'r', ... % 1-sigma upper-bound
    timeVector, -sqrt(PCorrected(:,1,1)),'r');   % 1-sigma lower-bound
xlabel('Time [s]');
ylabel('Error for state 1');
title('State estimation errors');
subplot(2,1,2);
plot(timeVector,eStates(:,2),...               % Error for the second state
    timeVector,sqrt(PCorrected(:,2,2)),'r', ...  % 1-sigma upper-bound
    timeVector,-sqrt(PCorrected(:,2,2)),'r');    % 1-sigma lower-bound
xlabel('Time [s]');
ylabel('Error for state 2');
legend('State estimate','1-sigma uncertainty bound',...
    'Location','Best');

Граница ошибки для состояния 1 приближается к 0 на t = 2.15 секунд из-за модели датчика (MeasurementFcn). Имеет форму $$x_1[k]*(1+v[k])$$. На t = 2,15 секунде истинное и оцененное состояния близки к нулю, что подразумевает, что ошибка измерения в абсолютных терминах также близка к нулю. Это отражено в ковариации ошибки расчета состояния фильтра.

Вычислим, какой процент точек выходит за рамки 1-сигма неопределенности.

distanceFromBound1 = abs(eStates(:,1))-sqrt(PCorrected(:,1,1));
percentageExceeded1 = nnz(distanceFromBound1>0) / numel(eStates(:,1));
distanceFromBound2 = abs(eStates(:,2))-sqrt(PCorrected(:,2,2));
percentageExceeded2 = nnz(distanceFromBound2>0) / numel(eStates(:,2));
[percentageExceeded1 percentageExceeded2]

ans = 1×2

    0.1386         0

Первые ошибки расчета состояния превышают $$1\sigma$$ неопределенность связывает приблизительно 14% временных шагов. Менее 30% ошибок, превышающих связанную с неопределенностью 1 сигма, подразумевают хорошую оценку. Этот критерий удовлетворяется для обоих состояний. Процент 0% для второго состояния означает, что фильтр консервативен: скорее всего, объединенные шумы процесса и измерения слишком высоки. Вероятно, лучшая эффективность может быть получена путем настройки этих параметров.

Вычислите среднюю автокорреляцию ошибок расчета состояния. Также постройте график автокорреляции.

mean(eStates)

ans = 1×2

   -0.0103    0.0201

[xeStates1,xeStatesLags1] = xcorr(eStates(:,1),'coeff'); % 'coeff': normalize by the value at zero lag
[xeStates2,xeStatesLags2] = xcorr(eStates(:,2),'coeff'); % 'coeff'
% Only plot non-negative lags
idx = xeStatesLags1>=0;
figure();
subplot(2,1,1);
plot(xeStatesLags1(idx),xeStates1(idx));
xlabel('Lags');
ylabel('For state 1');
title('Normalized autocorrelation of state estimation error');
subplot(2,1,2);
plot(xeStatesLags2(idx),xeStates2(idx));
xlabel('Lags');
ylabel('For state 2');

Среднее значение ошибок мало относительно значения состояний. Автокорреляция ошибок расчета состояния показывает мало неслучайных изменения для малых значений задержки, но они намного меньше нормализованного пикового значения 1. В сочетании с тем, что предполагаемые состояния являются точными, такое поведение невязок может быть рассмотрено как удовлетворительные результаты.

Конструкция фильтра частиц

Неароматизированные и расширенные фильтры Калмана направлены на отслеживание среднего и ковариационного апостериорного распределения оценок состояния различными методами приближения. Эти методы могут быть недостаточными, если нелинейности в системе являются серьезными. В сложение для некоторых приложений просто отслеживание среднего и ковариации апостериорного распределения оценок состояния может быть недостаточным. Фильтр частиц может решить эти проблемы, отслеживая эволюцию многих гипотез состояния (частиц) с течением времени за счет более высоких вычислительных затрат. Вычислительная стоимость и точность оценки увеличивается с количеством частиц.

The particleFilter команда в Control System Toolbox реализует алгоритм фильтра частиц в дискретном времени. В этом разделе вы проходите через построение particleFilter для тех же осцилляторов Ван дер Поля, используемых ранее в этом примере, и подсветок сходства и различия с сигма-точечным фильтром Калмана.

Функция перехода к состоянию, которую вы предоставляете particleFilter необходимо выполнить две задачи. Первый, выборка технологического шума из любого распределения, соответствующего вашей системе. Два, вычисляющие время распространения всех частиц (гипотезы о состоянии) к следующему шагу, включая эффект технологического шума, вычисленный вами на шаге 1.

type vdpParticleFilterStateFcn

function particles = vdpParticleFilterStateFcn(particles) 
% vdpParticleFilterStateFcn Example state transition function for particle
%                           filter
%
% Discrete-time approximation to van der Pol ODEs for mu = 1. 
% Sample time is 0.05s.
%
% predictedParticles = vdpParticleFilterStateFcn(particles)
%
% Inputs:
%    particles - Particles at current time. Matrix with dimensions
%                [NumberOfStates NumberOfParticles] matrix
%
% Outputs:
%    predictedParticles - Predicted particles for the next time step
%
% See also particleFilter

%   Copyright 2017 The MathWorks, Inc.

%#codegen

% The tag %#codegen must be included if you wish to generate code with 
% MATLAB Coder.

[numberOfStates, numberOfParticles] = size(particles);
    
% Time-propagate each particle
%
% Euler integration of continuous-time dynamics x'=f(x) with sample time dt
dt = 0.05; % [s] Sample time
for kk=1:numberOfParticles
    particles(:,kk) = particles(:,kk) + vdpStateFcnContinuous(particles(:,kk))*dt;
end

% Add Gaussian noise with variance 0.025 on each state variable
processNoise = 0.025*eye(numberOfStates);
particles = particles + processNoise * randn(size(particles));
end

function dxdt = vdpStateFcnContinuous(x)
%vdpStateFcnContinuous Evaluate the van der Pol ODEs for mu = 1
dxdt = [x(2); (1-x(1)^2)*x(2)-x(1)];
end

Существуют различия между функцией перехода между состояниями, которую вы снабжаете unscentedKalmanFilter и particleFilter. Функция перехода к состоянию, которую вы использовали для сигма-точечного фильтра Калмана, только что описала распространение одной гипотезы состояния на следующий временной шаг вместо набора гипотез. В сложение распределение шума процесса было задано в ProcessNoise свойство unscentedKalmanFilter, просто по ковариации. particleFilter может рассматривать произвольные распределения, которые могут потребовать определения большего количества статистических свойств. Это произвольное распределение и его параметры полностью заданы в функции перехода состояния, которую вы предоставляете particleFilter.

Функция правдоподобия измерения, которую вы предоставляете particleFilter необходимо также выполнить две задачи. Один, вычисляющий гипотезы измерений из частиц. Два, вычисление вероятности каждой частицы из измерения датчика и гипотез, вычисленных на этапе 1.

type vdpExamplePFMeasurementLikelihoodFcn

function likelihood = vdpExamplePFMeasurementLikelihoodFcn(particles,measurement)
% vdpExamplePFMeasurementLikelihoodFcn Example measurement likelihood function
%
% The measurement is the first state.
%
% likelihood = vdpParticleFilterMeasurementLikelihoodFcn(particles, measurement)
%
% Inputs:
%    particles - NumberOfStates-by-NumberOfParticles matrix that holds 
%                the particles
%
% Outputs:
%    likelihood - A vector with NumberOfParticles elements whose n-th
%                 element is the likelihood of the n-th particle
%
% See also extendedKalmanFilter, unscentedKalmanFilter

%   Copyright 2017 The MathWorks, Inc.

%#codegen

% The tag %#codegen must be included if you wish to generate code with 
% MATLAB Coder.

% Validate the sensor measurement
numberOfMeasurements = 1; % Expected number of measurements
validateattributes(measurement, {'double'}, {'vector', 'numel', numberOfMeasurements}, ...
    'vdpExamplePFMeasurementLikelihoodFcn', 'measurement');

% The measurement is first state. Get all measurement hypotheses from particles
predictedMeasurement = particles(1,:);

% Assume the ratio of the error between predicted and actual measurements
% follow a Gaussian distribution with zero mean, variance 0.2
mu = 0; % mean
sigma = 0.2 * eye(numberOfMeasurements); % variance

% Use multivariate Gaussian probability density function, calculate
% likelihood of each particle
numParticles = size(particles,2);
likelihood = zeros(numParticles,1);
C = det(2*pi*sigma) ^ (-0.5);
for kk=1:numParticles
    errorRatio = (predictedMeasurement(kk)-measurement)/predictedMeasurement(kk);
    v = errorRatio-mu;
    likelihood(kk) = C * exp(-0.5 * (v' / sigma * v) );
end
end

Теперь создайте фильтр и инициализируйте его с 1000 частицами вокруг среднего значения [2; 0] с 0,01 ковариацией. Ковариация небольшая, потому что у вас высокое доверие в своем предположении [2; 0].

pf = particleFilter(@vdpParticleFilterStateFcn,@vdpExamplePFMeasurementLikelihoodFcn);
initialize(pf, 1000, [2;0], 0.01*eye(2));

Опционально выберите метод оценки состояния. Это задается функцией StateEstimationMethod свойство particleFilter, который может взять значение 'mean' (по умолчанию) или 'maxweight'. Когда StateEstimationMethod является 'mean'объект извлекает средневзвешенное значение частиц из Particles и Weights свойства как оценка состояния. 'maxweight' соответствует выбору частицы (гипотеза состояния) с максимальным значением веса в Weights как оценка состояния. Кроме того, вы можете получить доступ к Particles и Weights свойства объекта и извлечь вашу оценку состояния произвольным методом по вашему выбору.

pf.StateEstimationMethod

ans = 
'mean'

particleFilter позволяет задавать различные опции повторной дискретизации через его ResamplingPolicy и ResamplingMethod свойства. Этот пример использует настройки по умолчанию в фильтре. Смотрите particleFilter документация для получения дополнительной информации о повторной дискретизации.

pf.ResamplingMethod

ans = 
'multinomial'

pf.ResamplingPolicy

ans = 
  particleResamplingPolicy with properties:

                TriggerMethod: 'ratio'
             SamplingInterval: 1
    MinEffectiveParticleRatio: 0.5000

Запустите цикл оценки. Это представляет измерения, поступающие со временем, шаг за шагом.

% Estimate
xCorrectedPF = zeros(size(xTrue));
for k=1:size(xTrue,1)
    % Use measurement y[k] to correct the particles for time k
    xCorrectedPF(k,:) = correct(pf,yMeas(k)); % Filter updates and stores Particles[k|k], Weights[k|k]
    % The result is x[k|k]: Estimate of states at time k, utilizing
    % measurements up to time k. This estimate is the mean of all particles
    % because StateEstimationMethod was 'mean'.
    %
    % Now, predict particles at next time step. These are utilized in the
    % next correct command
    predict(pf); % Filter updates and stores Particles[k+1|k]
end

Постройте график оценок состояния из фильтра частиц:

figure();
subplot(2,1,1);
plot(timeVector,xTrue(:,1),timeVector,xCorrectedPF(:,1),timeVector,yMeas(:));
legend('True','Particlte filter estimate','Measured')
ylim([-2.6 2.6]);
ylabel('x_1');
subplot(2,1,2);
plot(timeVector,xTrue(:,2),timeVector,xCorrectedPF(:,2));
ylim([-3 1.5]);
xlabel('Time [s]');
ylabel('x_2');

Верхний график показывает истинное значение, оценку фильтра частиц и измеренное значение первого состояния. Фильтр использует системную модель и информацию о шуме, чтобы получить улучшенную оценку по сравнению с измерениями. Нижний график показывает второе состояние. Фильтр успешно дает хорошую оценку.

Валидация эффективности фильтра частиц включает в себя выполнение статистических тестов на невязках, аналогичных тем, которые были выполнены ранее в этом примере для результатов сигма-точечного фильтра Калмана.

Сводные данные

Этот пример показал шаги построения и использования сигма-точечного фильтра Калмана и фильтра частиц для оценки состояния нелинейной системы. Вы оценили состояния осциллятора Ван дер Поля по шумным измерениям и подтвердили эффективность оценки.

rmpath(fullfile(matlabroot,'examples','control','main')) % remove example data