Введение

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

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

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

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

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

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

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

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

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

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

Создайте эту функцию перехода состояния и сохраните ее в файле с именем vdpStateFcn.m. Используйте время выборки $${}_{\mathrm{Ts}}\mathrm{=}\mathrm{}\mathrm{}$$0,05 с. Вы предоставляете эту функцию 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. Эти входы могут отличаться от входов в функции перехода состояния.

Для этого примера предположим, что у вас есть измерения первого состояния $${}_{\mathrm{x1}}$$ в пределах некоторой процентной ошибки:

Это относится к категории неаддитивного шума измерения, поскольку шум измерения не просто добавляется к функции состояний. Вы хотите оценить и $${}_{\mathrm{x1}}$$, и $${}_{\mathrm{x2}}$$ по шумным измерениям.

Создайте эту функцию перехода состояния и сохраните ее в файле с именем 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;

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

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

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

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

Имитируйте генератор van der Pol в течение 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 в панели инструментов обработки сигналов.

[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. Средняя корреляция близка к нулю, и корреляция не показывает каких-либо значительных неслучайных вариаций. Эти характеристики повышают уверенность в производительности фильтра.

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

Во-первых, взгляните на погрешность и $$\mathrm{}$$границы неопределенности 1λ из оценки ковариации ошибки фильтра.

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). Имеет вид $${}_{\mathrm{x1}}[k]\mathrm{*}(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

Ошибки оценки первого состояния превышают $$\mathrm{}$$погрешность 1, связанную приблизительно с 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. В сочетании с тем, что оценочные состояния являются точными, такое поведение остатков можно рассматривать как удовлетворительные результаты.

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

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

particleFilter команда в панели инструментов системы управления реализует алгоритм дискретного фильтра частиц. В этом разделе рассматривается создание particleFilter для того же самого van der Pol осциллятора, использованного ранее в этом примере, и подчеркивает сходства и различия с незаметным фильтром Калмана.

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

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 также должны выполнять две задачи. Во-первых, вычисление гипотез измерения из частиц. Во-вторых, вычисление вероятности каждой частицы из измерения датчика и гипотез, вычисленных на первом этапе.

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