В этом примере показано, как выполнить Кальмана, фильтрующего. Во-первых, вы проектируете установившийся фильтр с помощью kalman
команда. Затем вы симулируете систему, чтобы показать, как она уменьшает ошибку от шума измерения. Этот пример также показывает, как реализовать изменяющийся во времени фильтр, который может быть полезен для систем с неустановившимися источниками шума.
Рассмотрите следующий дискретный объект с Гауссовым шумом w на входе и шуме измерения v на выходе:
Цель состоит в том, чтобы спроектировать Фильтр Калмана, чтобы оценить истинный объект выход на основе шумных измерений . Этот установившийся Фильтр Калмана использует следующие уравнения для этой оценки.
Обновление времени:
Обновление измерения:
Здесь,
оценка , учитывая прошлые измерения до .
и предполагаемые значения состояния и измерение, обновленное на основе последнего измерения .
и оптимальные инновационные усиления, выбранные, чтобы минимизировать установившуюся ковариацию ошибки расчета, учитывая шумовые ковариации , , и . (Для получения дополнительной информации о том, как эти усиления выбраны, смотрите kalman
.)
(Эти уравнения обновления описывают current
введите средство оценки. Для получения информации о различии между current
средства оценки и delayed
средства оценки, смотрите kalman
.)
Можно использовать kalman
функционируйте, чтобы спроектировать этот установившийся Фильтр Калмана. Эта функция решает, что оптимальный установившийся фильтр получает M для конкретного объекта на основе ковариации шума процесса Q и ковариации шума датчика R, что вы обеспечиваете. В данном примере используйте следующие значения для матриц пространства состояний объекта.
A = [1.1269 -0.4940 0.1129 1.0000 0 0 0 1.0000 0]; B = [-0.3832 0.5919 0.5191]; C = [1 0 0]; D = 0;
В данном примере установите G = B, подразумевая, что шум процесса w является аддитивным входным шумом. Кроме того, набор H = 0, означая, что входной шум w не оказывает прямого влияния на выход y. Эти предположения дают к более простой модели объекта управления:
Когда H = 0, этому можно показать это (см. kalman
). Вместе, эти предположения также упрощают уравнения обновления для Фильтра Калмана.
Обновление времени:
Обновление измерения:
Чтобы спроектировать этот фильтр, сначала создайте модель объекта управления с входом для w
. Установите шаг расчета на -1
отметить объект как дискретный (без определенного шага расчета).
Ts = -1; sys = ss(A,[B B],C,D,Ts,'InputName',{'u' 'w'},'OutputName','y'); % Plant dynamics and additive input noise w
Ковариация шума процесса Q
и ковариация шума датчика R
значения, больше, чем нуль, что вы обычно получаете из исследований или измерений вашей системы. В данном примере задайте следующие значения.
Q = 2.3; R = 1;
Используйте kalman
команда, чтобы спроектировать фильтр.
[kalmf,L,~,Mx,Z] = kalman(sys,Q,R);
Эта команда проектирует Фильтр Калмана, kalmf
, модель в пространстве состояний, которая реализует уравнения обновления времени и обновления измерения. Входные параметры фильтра являются входным u объекта, и шумный объект вывел y. Первый выход kalmf
оценка из истинного объекта выход и остающиеся выходные параметры являются оценками состояния .
В данном примере отбросьте оценки состояния и сохраните только первый выход, .
kalmf = kalmf(1,:);
Чтобы видеть, как этот фильтр работает, сгенерируйте некоторые данные и сравните отфильтрованный ответ с истинным ответом объекта. Полную систему показывают в следующей схеме.
Чтобы симулировать эту систему, используйте sumblk
создать вход для шума измерения v
. Затем используйте connect
соединить sys
и Фильтр Калмана, вместе таким образом, что u
разделяемый вход и шумный объект выход y
подача в другой фильтр вводится. Результатом является имитационная модель с входными параметрами w
V
, и u
и выходные параметры yt
(истинный ответ) и ye
(отфильтрованный или предполагаемый ответ ). Сигналы yt
и ye
выходные параметры объекта и фильтра, соответственно.
sys.InputName = {'u','w'}; sys.OutputName = {'yt'}; vIn = sumblk('y=yt+v'); kalmf.InputName = {'u','y'}; kalmf.OutputName = 'ye'; SimModel = connect(sys,vIn,kalmf,{'u','w','v'},{'yt','ye'});
Чтобы симулировать поведение фильтра, сгенерируйте известный синусоидальный входной вектор.
t = (0:100)'; u = sin(t/5);
Сгенерируйте шум процесса и векторы шума датчика с помощью тех же шумовых значений ковариации Q
и R
то, что вы раньше проектировали фильтр.
rng(10,'twister');
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);
Наконец, симулируйте ответ с помощью lsim
.
out = lsim(SimModel,[u,w,v]);
lsim
генерирует ответ при выходных параметрах yt
и Вы к входным параметрам, примененным в w
V
, и u
. Извлеките yt
и Вы образовываете канал, и вычислите измеренный отклик.
yt = out(:,1); % true response ye = out(:,2); % filtered response y = yt + v; % measured response
Сравните истинный ответ с отфильтрованным ответом.
clf subplot(211), plot(t,yt,'b',t,ye,'r--'), xlabel('Number of Samples'), ylabel('Output') title('Kalman Filter Response') legend('True','Filtered') subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'), xlabel('Number of Samples'), ylabel('Error') legend('True - measured','True - filtered')
Когда второй график показывает, Фильтр Калмана уменьшает ошибку yt - y
из-за шума измерения. Чтобы подтвердить это сокращение, вычислите ковариацию ошибки прежде, чем отфильтровать (ковариация погрешности измерения) и после фильтрации (ковариация ошибки расчета).
MeasErr = yt-yt; MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0
EstErr = yt-ye; EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479
Предыдущий проект принял, что шумовые ковариации не изменяются в зависимости от времени. Изменяющийся во времени Фильтр Калмана может выполнить хорошо, даже когда шумовая ковариация не является стационарной.
Изменяющийся во времени Фильтр Калмана имеет следующие уравнения обновления. В изменяющемся во времени фильтре, оба ошибочная ковариация и инновационное усиление может меняться в зависимости от времени. Можно изменить время и уравнения обновления измерения с учетом изменения времени можно следующим образом. (См. kalman
для большего количества детали об этих выражениях.)
Обновление времени:
Обновление измерения:
Можно реализовать изменяющийся во времени Фильтр Калмана в Simulink® с помощью блока Kalman Filter. Для примера, демонстрирующего использование того блока, смотрите, что Оценка состояния Использует Изменяющийся во времени Фильтр Калмана. В данном примере реализуйте изменяющийся во времени фильтр в MATLAB®.
Чтобы создать изменяющийся во времени Фильтр Калмана, во-первых, генерируют шумный ответ объекта. Симулируйте ответ объекта на входной сигнал u
и шум процесса w
заданный ранее. Затем добавьте шум измерения v
к симулированному истинному ответу yt
получить шумный ответ y
. В этом примере, ковариациях шумовых векторов w
и v
не изменяйтесь со временем. Однако можно использовать ту же процедуру для неустановившегося шума.
yt = lsim(sys,[u w]); y = yt + v;
Затем реализуйте рекурсивные уравнения обновления фильтра в for
цикл.
P = B*Q*B'; % Initial error covariance x = zeros(3,1); % Initial condition on the state ye = zeros(length(t),1); ycov = zeros(length(t),1); errcov = zeros(length(t),1); for i=1:length(t) % Measurement update Mxn = P*C'/(C*P*C'+R); x = x + Mxn*(y(i)-C*x); % x[n|n] P = (eye(3)-Mxn*C)*P; % P[n|n] ye(i) = C*x; errcov(i) = C*P*C'; % Time update x = A*x + B*u(i); % x[n+1|n] P = A*P*A' + B*Q*B'; % P[n+1|n] end
Сравните истинный ответ с отфильтрованным ответом.
subplot(211), plot(t,yt,'b',t,ye,'r--') xlabel('Number of Samples'), ylabel('Output') title('Response with Time-Varying Kalman Filter') legend('True','Filtered') subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'), xlabel('Number of Samples'), ylabel('Error') legend('True - measured','True - filtered')
Изменяющийся во времени фильтр также оценивает выходную ковариацию во время оценки. Поскольку этот пример использует стационарный входной шум, выходная ковариация стремится к установившемуся значению. Постройте выходную ковариацию, чтобы подтвердить, что фильтр достиг устойчивого состояния.
figure plot(t,errcov) xlabel('Number of Samples'), ylabel('Error Covariance'),
Из графика ковариации вы видите, что выходная ковариация достигает устойчивого состояния приблизительно в пяти выборках. С тех пор изменяющийся во времени фильтр имеет ту же эффективность как установившаяся версия.
Как в установившемся случае, фильтр уменьшает ошибку из-за шума измерения. Чтобы подтвердить это сокращение, вычислите ковариацию ошибки прежде, чем отфильтровать (ковариация погрешности измерения) и после фильтрации (ковариация ошибки расчета).
MeasErr = yt - y; MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0.9871
EstErr = yt - ye; EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479
Наконец, когда изменяющийся во времени фильтр достигает устойчивого состояния, значений в матрице усиления Mxn
совпадайте с вычисленными kalman
для установившегося фильтра.
Mx,Mxn
Mx = 3×1
0.5345
0.0101
-0.4776
Mxn = 3×1
0.5345
0.0101
-0.4776