Введение

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

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

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

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

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

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

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

Обозначьте это уравнение как $$\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. Эти входные параметры могут отличаться, чем входные параметры в функции изменения состояния.

В данном примере примите, что у вас есть измерения первого состояния $$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;

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

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

Оценка Используя предсказывает и корректирует Команды

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

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

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

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

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

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

Функция изменения состояния вы обеспечиваете к 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

Теперь создайте фильтр и инициализируйте его 1 000 частиц вокруг среднего значения [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