Этот пример показов, как использовать алгоритмы сигма-точечного фильтра Калмана и фильтра частиц для нелинейной оценки состояния для осциллятора Ван дер Поля.
Этот пример также использует Signal Processing Toolbox™.
System Identification Toolbox™ предлагает три команды для нелинейной оценки состояния:
extendedKalmanFilter
: Фильтр Калмана первого порядка в дискретном времени
unscentedKalmanFilter
: Дискретный сигма-точечный фильтр Калмана
particleFilter
: Фильтр частиц в дискретном времени
Типичный рабочий процесс использования этих команд следующий:
Моделируйте поведение своего объекта и датчика.
Создайте и сконфигурируйте extendedKalmanFilter
, unscentedKalmanFilter
или particleFilter
объект.
Выполните оценку состояния при помощи predict
и correct
команды с объектом.
Анализируйте результаты, чтобы получить доверие в эффективности фильтра
Разверните фильтр на своем оборудовании. Сгенерировать код для этих фильтров можно с помощью MATLAB Coder™.
Этот пример сначала использует unscentedKalmanFilter
команда для демонстрации этого рабочего процесса. Затем демонстрирует использование particleFilter
.
Сигма-точечный фильтр Калмана (UKF) требует функции, которая описывает эволюцию состояний от одного временного шага до следующего. Обычно это называется функцией перехода состояния. unscentedKalmanFilter
поддерживает следующие две формы функций:
Аддитивный технологический шум:
Не аддитивный технологический шум:
Здесь f (..) - переходная функция состояния, x - состояние, w - шум процесса. u является необязательным и представляет дополнительные входы f, для образца входов или параметров системы. u может быть задано как нуль или больше аргументов функции. Аддитивный шум означает, что состояние и технологический шум связаны линейно. Если отношение нелинейно, используйте вторую форму. Когда вы создаете unscentedKalmanFilter
объект, вы задаете f (..), а также является ли технологический шум аддитивным или не аддитивным.
Система в этом примере является осциллятором Ван дер Поля с mu = 1. Эта система с 2 состояниями описывается следующим набором нелинейных обыкновенных дифференциальных уравнений (ОДУ):
Обозначите это уравнение как , где . Шум процесса w не появляется в системную модель. Можно предположить, что это добавка для простоты.
unscentedKalmanFilter
требует функции перехода состояния в дискретном времени, но модель объекта управления непрерывна. Можно использовать приближение в дискретном времени к модели в непрерывном времени. Дискретизация Эйлера является одним из распространенных методов приближения. Предположим, что ваш шаг расчета , и обозначить динамику в непрерывном времени, которую вы имеете как . Дискретизация Эйлера аппроксимирует оператор производной как . Получившаяся функция перехода состояния в дискретном времени является:
Точность этого приближения зависит от шага расчета . Меньше значения обеспечивают лучшие приближения. Кроме того, можно использовать другой метод дискретизации. Например, семейство методов Рунге-Кутты более высокого порядка обеспечивает более высокую точность за счет больших вычислительных затрат, учитывая фиксированный шаг расчета .
Создайте эту функцию перехода к состоянию и сохраните ее в файле с именем vdpStateFcn.m. Использование шага расчета . Вы предоставляете эту функцию unscentedKalmanFilter
во время конструкции объекта.
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 термином. Эти входы могут отличаться от входов в функции перехода состояния.
В данном примере предположим, что у вас есть измерения первого состояния в пределах некоторой процентной ошибки:
Это попадает в категорию неаддитивного измерения шума, потому что измерение шум не просто добавляется к функции состояний. Вы хотите оценить оба и от шумных измерений.
Создайте эту функцию перехода и сохраните ее в файле с именем vdpMeasurementNonAdditiveNoiseFcn.m. Вы предоставляете эту функцию unscented
во время конструкции объекта.
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. Средняя корреляция близка к нулю, и корреляция не показывает каких-либо значительных неслучайных изменений. Эти характеристики повышают доверие в эффективности фильтра.
На самом деле истинные состояния никогда не доступны. Однако при выполнении симуляций вы имеете доступ к реальным состояниям и можете посмотреть на ошибки между предполагаемым и истинным состояниями. Эти ошибки должны удовлетворять критериям, подобным остаточным:
Малая величина
Отклонение в ковариационной оценке ошибки фильтра
Нулевое среднее
Некоррелированный.
Во-первых, посмотрите на ошибку и ограничения неопределенности из ковариационной оценки ошибки фильтра.
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). Имеет форму. На 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
Первые ошибки расчета состояния превышают неопределенность связывает приблизительно 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
команда в System Identification Toolbox реализует алгоритм фильтра частиц в дискретном времени. В этом разделе рассматривается построение фильтра частиц для того же осциллятора Ван дер Поля, используемого ранее в этом примере, и подчеркиваются сходства и различия с сигма-точечным фильтром Калмана.
Функция перехода к состоянию, которую вы предоставляете 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
. Функция перехода к состоянию, которую вы использовали для сигма-точечного фильтра Калмана, только что описала распространение одной гипотезы состояния на следующий временной шаг вместо набора гипотез. В сложение распределение шума процесса было задано в ProcessNois
свойство e unscentedKalmanFilter
, просто по ковариации. Фильтр частиц может рассматривать произвольные распределения, которые могут потребовать определения более статистических свойств. Это произвольное распределение и его параметры полностью заданы в функции перехода состояния, которую вы предоставляете 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');
Верхний график показывает истинное значение, оценку фильтра частиц и измеренное значение первого состояния. Фильтр использует системную модель и информацию о шуме, чтобы получить улучшенную оценку по сравнению с измерениями. Нижний график показывает второе состояние. Фильтр успешно дает хорошую оценку.
Валидация эффективности фильтра частиц включает в себя выполнение статистических тестов на невязках, аналогичных тем, которые были выполнены ранее в этом примере для результатов сигма-точечного фильтра Калмана.
Этот пример показал шаги построения и использования сигма-точечного фильтра Калмана и фильтра частиц для оценки состояния нелинейной системы. Вы оценили состояния осциллятора Ван дер Поля по шумным измерениям и подтвердили эффективность оценки.
extendedKalmanFilter
| particleFilter
| unscentedKalmanFilter