Фильтр Калмана: от простого к сложному / Хабр

Фильтра Калмана много не бывает! По этой теме издано несколько книг, опубликовано большое количество статей, в том числе на Хабре. Разработанный в 1960-х годах алгоритм оценки состояния динамических систем по сегодняшний день считается одним из лучших, получает все более широкое применение в различных технических системах: от радиолокации до электрокардиографии.

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

Все модели, которые я буду использовать и описывать, выполнены на языке Matlab – среде, изначально созданной для работы с матрицами. Гарантированно они будут работать на версии R2016b и выше.

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

Преимуществом фильтра Калмана является его способность опираться не только на измерения оцениваемой величины, но и на прогнозные значения, рассчитываемые в модели объекта. Например, если мы хотим оценить координаты летательного аппарата, мы будем больше опираться на данные GPS там, где сигнал стабилен – на открытой местности, и на расчетные данные там, где мы ожидаем повышенную зашумленность – при нахождении БПЛА в тоннеле.

Простой пример. Оценка температуры воздуха в помещении

Начнем с простейшего с точки зрения понимания физики процесса примера – оценки температуры в помещении. Что мы знаем о модели этого объекта? Кроме того, что температура, как правило, не изменяется скачками мгновенно, больше ничего. Если это "герметично" закрытая комната, нет открытых окон и дверей, сквозняков, то температура будет примерно постоянная.

На рисунке показана стандартная блок-схема, описывающими принцип работы фильтра Калмана. Алгоритм состоит из пяти уравнений. Поскольку мы работаем всего с одной переменной – температурой, все матрицы будут иметь размерность [1×1], то есть будут представлять собой постоянные величины (для нашего примера).

Выберем начальные значения. Поскольку мы еще не произвели ни одного измерения, мы можем только оценить по собственным ощущениям температуру в комнате. Да, для работы фильтра Калмана нужно понимать объект измерения и физику процессов в нём. Допустим, по нашим ощущениям в комнате сейчас $22 ^{\circ}$ C. То есть $x_{k-1} =22$ .

$P_{k-1}$ – это наше ожидание ошибки. Если мы находимся без верхней одежды в помещении, нам не холодно и не жарко, то вряд ли мы в своих ощущениях температуры ошибаемся больше, чем на три градуса. А вот если на глаз пытаемся оценить температуру расплавленного металла в домне, легко ошибиться и на 100 градусов.

Цикл состоит из двух шагов: 1 – предсказание, 2 – корректировка.

В процессе замера в помещении кто-то может открыть окно, впустив морозный зимний воздух, включить/выключить нагревательный прибор или кондиционер, открыть/закрыть дверь. Всё это непредсказуемые факторы управляющего воздействия на систему. Мы считаем, что их нет в нашем эксперименте, поэтому управляющее воздействие в предыдущий момент времени $u_{k-1}=0$ .

Тогда формула предсказания состояния системы упростится до $x_{kpred}=Fx_{k-1}$ , где F – матрица перехода между состояниями. Если мы предскажем: "сейчас 22°C", то через минуту будет примерно 22°C ± 0.1°C. Таким образом, примем F=1. Модель: "Температура в комнате почти не меняется".

Отсюда и предсказание ошибки ковариации: $P_{kpred}=FP_{k-1}F^T+Q.$ Это не просто ошибка, а наша уверенность в этой ошибке. Большое P означает, что мы сильно сомневаемся в своей оценке, малое P — мы почти уверены. Какой физический смысл имеют матрицы Q и R мы подробно рассмотрим чуть позже. F^T– транспонированная матрица F (в нашем случае тоже равна 1).

Шаг 2. Корректировка. На этом шаге происходит вычисление коэффициента Калмана K, который показывает, насколько нужно доверять новому измерению относительно предположения, полученного на первом шаге. Обновление оценки осуществляется по формуле $x_k=x_{kpred}+K(z_k-Hx_{kpred})$ . H – матрица измерений. Мы измеряем температуру напрямую, поэтому H=1. Разница $(z_k-Hx_{kpred})$ называется инновацией, её физический смысл мы тоже рассмотрим подробнее позже.

Код модели:

%% Параметры модели
dt = 1;              % Время между измерениями (сек)
N = 50;              % Количество шагов

% Истинная температура (неизвестна в реальности)
true_temp = 21 + 0.5*randn(1,N); % Постоянная температура с шумом
% Здесь 0.5 - это не шум измерений, а естественная флуктуация "истинной" температуры

% Измерения (зашумленные)
measurement_noise = 3; % Погрешность термометра (°C)
measurements = true_temp + measurement_noise*randn(1,N);

%% Параметры фильтра Калмана
F = 1;  % Температура остается примерно постоянной
H = 1;  % Измеряем температуру напрямую

% Ковариации (меры неопределенности)
Q = 0.01;  % Неопределенность модели (насколько плоха наша модель)
R = 9;     % Неопределенность измерений (дисперсия шума термометра)

% Начальные условия
x_est = 22;      % Начальная оценка температуры
P_est = 1;       % Начальная неопределенность оценки

%% Алгоритм фильтра Калмана
estimates = zeros(1, N);  % Массив для хранения оценок
kalman_gains = zeros(1, N); % Массив для коэффициентов Калмана

figure('Position', [100, 100, 1200, 500]);

for k = 1:N
    % --- ПРЕДСКАЗАНИЕ ---
    % Предсказываем следующее состояние по модели
    x_pred = F * x_est;
    % Предсказываем неопределенность
    P_pred = F * P_est * F' + Q;
    
    % --- КОРРЕКЦИЯ  ---
    % Получаем измерение
    z = measurements(k);
    
    % Вычисляем коэффициент Калмана (обновляется на каждом шаге!)
    K = P_pred * H' / (H * P_pred * H' + R);
    kalman_gains(k) = K;
    
    % Корректируем оценку с учетом измерения
    x_est = x_pred + K * (z - H * x_pred);
    
    % Корректируем неопределенность
    P_est = (1 - K * H) * P_pred;
    
    % Сохраняем оценку
    estimates(k) = x_est;
end

Визуализация работы:

Оценка температуры и коэффициент Калмана в динамике

Конкретно для этого примера мы получили:
- Средняя ошибка измерений: 2.48°C
- Средняя ошибка фильтра Калмана: 0.52°C
- Эффект: 78.9%

Как мы видим из кода, для расчета мы выбрали Q=0,01 и R=9. Почему именно такие?
Q — "Неточность нашей модели". Чем оно меньше, тем больше мы доверяем нашей модели.
R — "Неточность термометра". Чем он выше, тем выше неточность термометра. R = 9 означает, что наш термометр довольно неточный, что справедливо для дешевого бытового прибора, работающего с погрешностью ±3°C (стандартное отклонение шума $\sigma=\sqrt{R}=\sqrt9 = 3°C$ . Подробнее о нормальном распределении и его значении в работе фильтра Калмана можно прочитать здесь).

Почему мы выбрали именно такое соотношение? Вернемся к формуле расчета коэффициента Калмана. При H=1 формула примет вид:

$K=\frac{P_{pred}}{P_{pred}+R}$ .

Если модель точная (R велико), K → 0, и мы больше доверяем модели. Вместе с тем, при малых Q мы также больше доверяем модели потому что $P_{kpred}=FP_{k-1}F^T+Q$ , а значит K → 0. В нашем примере ("установившееся" значение коэффициента Калмана на правом графике) K≈0,04 – доверяем модели на 96%, измерениям на 4%.

Для нашего эксперимента логично соотношение Q : R ≈ 1 : 100 или 1 : 1000, так как температура меняется в 100-1000 раз медленнее, чем шумит термометр.

Что было бы в противоположном случае?

Q = 9;    % Модель неточная (температура скачет)
R = 0.01; % Термометр очень точный (лабораторный)

K ≈ 0.99. Доверяем измерениям на 99%, фильтр почти копирует показания термометра. Такое сочетание следовало бы выбрать для эксперимента, где к��мната с открытым окном (сквозняк), а термометр используется очень точный, лабораторный.

А если температура в помещении начнет расти?

Предположим, в нашей "герметичной" комнате в летний день за счет солнца начал проявляться парниковый эффект. Температура начнёт расти. Справится ли фильтр в таком случае?

Добавим в функцию истиной температуры восходящий тренд:

true_temp = 22 + 0.1*(1:N) + 0.5*randn(1,N); % Медленно растет с шумом

Не меняя модели объекта (F=1, "Температура не меняется"), немного увеличим доверие к измерениям, снизим доверие к модели (Q=0.1; R=5). Флуктуации оставим на прежнем уровне. Получим результат:

Работа фильтра при росте температуры (F=1)

Средняя ошибка измерений: 2.48°C
Средняя ошибка фильтра Калмана: 0.87°C
Эффект: 65.1%

Оценка скорости вращения вала двигателя постоянного тока

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

Модель основана на уравнении движения для двигателя постоянного тока:

$J·dω/dt + b·ω = K_t·I - T_{load}$

где:
— момент инерции;
b — коэффициент трения;
— коэффициент момента (зависимость от тока);
— коэффициент противо-ЭДС;
— сопротивление обмотки;
$T_{load}$ — статический момент нагрузки.

Предсказание значения измеряемой величины в дискретной модели выглядит следующим образом:

, где

— матрица перехода между состояниями (аналог матрицы F из первого примера), описывает инерционность системы: как механическую, так и электромеханическую и электрическую;

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

Подробнее о том, как получились эти уравнения

В непрерывной модели электрическое равновесие цепи якоря описывается уравнением:

где – напряжение на якоре (В), – ток якоря (А), – угловая скорость ротора (рад/с), R – сопротивление обмотки (Ом), – коэффициент противо-ЭДС (В·с/рад).

Механическое уравнение движения:

$J\frac{dω(t)}{dt}+bω(t)=K_ti(t),$

где J – момент инерции ротора (кг·м²), b – коэффициент вязкого трения (Н·м·с), – коэффициент момента (Н·м/А).

Выразим из первого уравнения ток:

$i(t)=\frac{U(t)−K_eω(t)}{R}$ и подставим его во второе уравнение.

Раскроем скобки, приведем подобные, перенесем скорость в левую часть и получим:

$J\frac{dω}{dt}+(b+\frac{K_tK_e}{R})ω=\frac{K_t}{R}U.$

Для работы в цифровом фильтре Калмана необходимо перейти к дискретному времени с шагом . Применяя простейшую аппроксимацию производной методом Эйлера (прямая разность):

$\frac{dw}{dt}≈\frac{w_{k+1}-w_k}{T_s},$

получим $J\frac{w_{k+1}-w_k}{T_s}+(b+\frac{K_tK_e}{R})ω=\frac{K_t}{R}U.$

Выразим $w_{k+1}$ и получим уравнение состояния в формате:

$ω_{k+1}=Aω_k+BU_k,$ где

$A=1-T_s(\frac{b}{J}+\frac{K_tK_e}{JR}),$ $B=T_s\frac{K_t}{JR}.$

В коде Matlab для простоты обозначено как "dt".

На первом шаге цикла (предсказание) используем физическую модель двигателя (в сравнении с первым примером измерения температуры теперь мы учитываем управляющее воздействие и не равно нулю:

x_pred = A * x_est + B * U(k);      
P_pred = A * P_est * A' + Q;

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

Код Matlab

%% ПАРАМЕТРЫ МОДЕЛИ ДВИГАТЕЛЯ
dt = 0.02;          % Время дискретизации (20 мс)
T = 10;             % Общее время моделирования
N = T/dt;           % Количество шагов

% Физические параметры двигателя постоянного тока
J = 0.02;           % Момент инерции (кг·м²)
b = 0.1;            % Коэффициент вязкого трения (Н·м·с/рад)
Kt = 0.5;           % Коэффициент момента (Н·м/А)
Ke = 0.5;           % Коэффициент противо-ЭДС (В·с/рад)
R = 2;              % Сопротивление обмотки (Ом)

% Дискретная модель состояния: ω(k+1) = A·ω(k) + B·U(k)
A = 1 - dt*(b/J + Kt*Ke/(J*R));  % Матрица состояния
B = dt*(Kt/(J*R));               % Матрица управления

%% ГЕНЕРАЦИЯ ТЕСТОВЫХ ДАННЫХ
time = (0:N-1)*dt;
true_speed = zeros(1, N);       % Истинная скорость
U = zeros(1, N);                % Управляющее напряжение

% Управление двигателем (ступенчатое напряжение)
for i = 1:N
    if i < N/5
        U(i) = 6;       % Разгон
    elseif i < 2*N/5
        U(i) = 10;      % Скорость 1
    elseif i < 3*N/5
        U(i) = 4;       % Скорость 2
    elseif i < 4*N/5
        U(i) = 8;       % Скорость 3
    else
        U(i) = 0;       % Выбег
    end
end

% Моделирование истинной скорости с учетом физики двигателя
for i = 2:N
    true_speed(i) = A*true_speed(i-1) + B*U(i-1);
end

% Добавляем шум процесса (случайные возмущения)
true_speed = true_speed + 0.1*randn(1,N);

% Генерируем зашумленные измерения
measurement_noise = 1.5;        % Стандартное отклонение шума датчика
measurements = true_speed + measurement_noise*randn(1,N);

%% НАСТРОЙКА ФИЛЬТРА КАЛМАНА
x_est = 0;                      % Начальная оценка скорости
P_est = 5;                      % Начальная неопределенность

Q = 0.01;                       % Ковариация шума процесса
R = measurement_noise^2;        % Ковариация шума измерений

% Массивы для хранения результатов
estimates = zeros(1, N);
kalman_gains = zeros(1, N);
predictions = zeros(1, N);

%% АЛГОРИТМ ФИЛЬТРА КАЛМАНА
for k = 1:N
    % ШАГ 1: ПРЕДСКАЗАНИЕ ПО МОДЕЛИ ДВИГАТЕЛЯ
    x_pred = A * x_est + B * U(k);
    P_pred = A * P_est * A' + Q;
    predictions(k) = x_pred;
    
    % ШАГ 2: КОРРЕКЦИЯ ПО ИЗМЕРЕНИЯМ
    z = measurements(k);
    
    % Вычисление коэффициента Калмана
    K = P_pred / (P_pred + R);
    kalman_gains(k) = K;
    
    % Обновление оценки
    x_est = x_pred + K * (z - x_pred);
    
    % Обновление ковариации
    P_est = (1 - K) * P_pred;
    
    estimates(k) = x_est;
end

Считаем модель точной (Q=0,01), измерения скорости тахогенератором с высокой погрешностью считаем неточными $(R=\sigma^2=2.25)$

Визуализация результатов оценки скорости ДПТ

В этом примере мы получили среднюю ошибку измерений 1,2 рад/с, ошибку на выходе фильтра Калмана — 0,13 рад/с.

Модификации алгоритма

Выше были рассмотрены простые примеры работы скалярного фильтра первого порядка. Для улучшения качества фильтрации – получения более сглаженного сигнала, приближенного к истинному значению, используются различные модификации фильтра. Чтобы не было путаницы, сразу скажу, что во всех примерах дальше мы будем использовать исключительно линейный (обычный) дискретный фильтр Калмана (Linear Kalman Filter). Существуют другие разновидности фильтра, классифициру��мые по математической модели его работы (расширенный, сигма-точечный, фильтр Калмана-Бьюси), их мы в данной статье не рассматриваем. Под модификациями здесь мы понимаем классификацию по архитектуре и настройке линейного фильтра: скалярный (описанный в двух примерах выше), векторные, каскадный, адаптивный.

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

Формируем сигнал заданной формы

clean_signal = zeros(size(t));

% 1. Синусоида (0 – 0.6 с)
idx_sin = t <= 0.6;
f_sin = 5;                      % Частота синуса, Гц
clean_signal(idx_sin) = sin(2*pi*f_sin*t(idx_sin));

% 2. Трапеция (0.6 – 1.4 с)
idx_trap = (t > 0.6) & (t <= 1.4);
T_total = 1.4 - 0.6;            % 0.8 с
T_phase = T_total / 3;          % 0.2667 с – длительность нарастания и спада
% Опорные точки трапеции
t_break = [0.6, 0.6+T_phase, 0.6+2*T_phase, 1.4];
v_break = [0, 1, 1, 0];
clean_signal(idx_trap) = interp1(t_break, v_break, t(idx_trap), 'linear');

% 3. Линейный тренд (1.4 – 2.0 с)
idx_lin = t > 1.4;
clean_signal(idx_lin) = 2 * (t(idx_lin) - 1.4);   % от 0 до 1.2

%% Добавление переменного шума (для проверки адаптивного фильтра)
noise_power = 0.5 * ones(size(t));
noise_power((t > 0.5) & (t <= 1.5)) = 2.0;   % средний шум
noise_power(t > 1.5) = 4.0;                   % высокий шум

Векторный фильтр второго и третьего порядка (многомерный фильтр Калмана)

До этого момента мы работали со скалярными величинами. Преимущество фильтра Калмана заключается ещё и в том, что мы можем отслеживать изменение не только одной координат��, а вектора состояния системы.

Фильтр второго порядка (положение + скорость)

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

x = [p; v]  где:
p - положение (position) - где я сейчас
v - скорость (velocity) -  как быстро движусь

В Matlab это выразится в матрицах перехода F и измерения H.

В этом примере мне было удобнее работать в Matlab с частотой дискретизации . Шаг дискретизации тогда определяется как .

F = [1, 1/Fs;   % Новая позиция = старая позиция + скорость*время. p=p0 + v*t
     0, 1];     % Новая скорость = старая скорость (предполагаем постоянной). v=v0

H = [1, 0];

Q = [0.02, 0;           % Меньший Q для положения
     0, 5];             % Большой Q для скорости (быстрая реакция на изменение)

Каков физический смысл этих уравнений?

Матрица $F=\begin{bmatrix} 1 & Δt \\ 0 & 1 \end{bmatrix}$ .

Пусть начальная ковариация диагональная: $P=\begin{bmatrix} \sigma_p^2 & 0 \\ 0 & \sigma_v^2 \end{bmatrix}$ — ошибки по положению и скорости независимы.

Вычислим $P_{pred}=FPF^T +Q.$ Для начала перемножим FP:

$FP=\begin{bmatrix} 1 & Δt \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \sigma_p^2 & 0 \\ 0 & \sigma_v^2 \end{bmatrix}=\begin{bmatrix} \sigma_p^2 & Δt\sigma_v^2 \\ 0 & \sigma_v^2 \end{bmatrix}$

Помножим полученное выражение на $F^T=\begin{bmatrix} 1 & 0 \\ Δt & 1 \end{bmatrix}$

Получим $FPF^T=\begin{bmatrix} \sigma_p^2 & Δt\sigma_v^2 \\ 0 & \sigma_v^2 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ Δt & 1 \end{bmatrix} = \begin{bmatrix} \sigma_p^2+Δt^2\sigma_v^2 & Δt\sigma_v^2 \\ Δt\sigma_v^2 & \sigma_v^2 \end{bmatrix}.$

Что мы видим?

Дисперсия положения увеличилась на $Δt^2\sigma_v^2$ — неопределенность растет со временем.
Появилась корреляция (недиагональные элементы) между положением и скоростью. Это логично, ошибка в скорости теперь влияет на положение.

Фильтр третьего порядка (положение + скорость + ускорение)

x = [p; v; a]  где:
p - положение
v - скорость  
a - ускорение

Тогда матрицы будут выглядеть следующим образом:

F = [1, 1/Fs, (1/Fs)^2/2;   % Позиция: p + v*t + 0.5*a*t²
     0, 1, 1/Fs;            % Скорость: v + a*t
     0, 0, 1];              % Ускорение: счиатем, остаётся прежним

Q = [0.01, 0, 0;   % Шум положения
     0, 0.1, 0;    % Шум скорости
     0, 0, 0.5];   % Шум ускорения
                   % Больше Q для ускорения = быстрее реагируем на изменение ускорения

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

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

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

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

Реализация в коде:

% Первый фильтр - БЫСТРЫЙ (грубая очистка)
R_fast = 0.3;              % Немного доверяем измерениям
Q_fast = [0.05, 0; 0, 2];  % Быстро реагирует на изменения

% Второй фильтр - МЕДЛЕННЫЙ (тонкая очистка)  
R_slow = 5.0;                % Почти не доверяем измерениям
Q_slow = [0.005, 0; 0, 0.1]; % Очень плавные изменения

Приведенные значения R и Q являются ориентировочными, требуют настройки в каждом конкретном случае. Основная рекомендация для настройки заключается в том, что для первого фильтра Q должен быть в 5...20 раз больше, чем R (быстро убираем 80% шума). Для второго прохода R должен быть в 5...20 раз больше, чем Q (аккуратно убираем оставшиеся 20% искажений).

Результат работы этого алгоритма будет показан дальше.

Адаптивный фильтр с автоматической настройкой

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

Адаптивный фильтр Калмана с автоматической настройкой — это интеллектуальная система, которая:

Наблюдает за своими ошибками (инновациями);
Анализирует тренды (дисперсию за время);
Принимает решения (меняет параметры);
Улучшает свою работу в реальном времени.

С чем это можно сравнить? Адаптивный регулятор — это автоматическая коробка передач в автомобиле — сама подбирает нужную передачу под скорость и нагрузку, в то время как обычный фильтр — это механика, где вы сами должны переключать передачи.

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

Почему именно R? Точность модели примерно постоянна во время работы. Создав её однажды, мы можем оценить доверие к ней. А вот точность измерений может существенно меняться в ходе работы. В начале статьи я упоминал применение фильтра Калмана в электрокардиографии. Во время процедуры снятия ЭКГ пациент может лежать спокойно, а может суетливо (ребенок). Естественно, во время движений пациента, шум измерений будет повышенный. Автоматическая подстройка R позволит адаптироваться к такому поведению.

Разберем пошагово реализацию адаптивного фильтра в коде.

Шаг 1. Инициализация параметров

function [filtered, R_hist] = filter_adaptive(signal, F, H, Q, x0, P0, R_init)

signal – зашумленный входной сигнал;
F, H, Q – матрицы модели (те же, что и для векторного фильтра второго порядка);
x0, P0 – начальные состояние и ковариация;
R_init – начальное значение ковариации измерения.

Внутри функции создаются:

x (2×n) – оценки состояния [положение; скорость];
P (2×2×n) – ковариации ошибки оценки;
R_hist – история изменения R;
innovation_history – буфер для хранения последних 10 инноваций (ошибок предсказания).

Шаг 2. Основной цикл адаптации

for k = 2:n
    % 1. Делаем предсказание (как обычно)
    x_pred = F * x(:,k-1);
    P_pred = F * P(:,:,k-1) * F' + Q;
    
    % 2. Вычисляем "инновацию" - разницу между предсказанием и измерением
    innovation = signal(k) - H * x_pred;
    
    % 3. ОБНОВЛЯЕМ ИСТОРИЮ ИННОВАЦИЙ
    innovation_history = [innovation; innovation_history(1:end-1)];
    % Добавляем текущую инновацию в начало, отбрасывая самую старую. 
    % Так хранятся последние 10 значений.

Инновация (innovation) — разница между реальным измерением и предсказанным значением по модели — значение, по которому фильтр оценивает, насколько он ошибся. При малых значениях инновации подстройка R не требуется, "всё идёт по плану". Большая разница между измеренной величиной и предсказанием является сигналом для системы, "что-то пошло не так".

Заметим, что мы оцениваем инновацию не в каждый момент времени, а более инертно — накопленную историю из десяти значений. На самом деле выбранное окно истории в 10 значений тоже является варьируемым для каждой конкретной задачи. Если мы возьмём маленькое окно, например, всего 5 значений, то получим более быструю реакцию на изменения. Однако тем самым мы снизим устойчивость системы на случайные выбросы. Если же мы будем оценивать инновации на интервале последних 20 значений, то получим более инерционный алгоритм, стабильную работу. Но реакция на изменения будет медленной.

Шаг 3. Адаптация параметра R

if k > 10
    innov_var = var(innovation_history);          % дисперсия последних 10 инноваций
    R_hist(k) = 0.1 + 2 * innov_var;              % эмпирическая формула
else
    R_hist(k) = R_hist(k-1);
end

Дисперсия инноваций innov_var отражает разброс ошибок предсказания. Если сигнал чистый, инновации малы и их дисперсия мала → можно уменьшить R (больше доверять измерениям). Если сигнал шумный, инновации велики, дисперсия большая → нужно увеличить R (меньше доверять шумным измерениям).

Формула R = 0.1 + 2 * innov_var – эмпирическая, подбирается для каждой конкретной задачи индивидуально. Слагаемое 0.1 гарантирует минимальное значение R (чтобы избежать нестабильности), а коэффициент 2 задаёт чувствительность к изменениям дисперсии. Для первых 10 шагов, пока нет достаточной статистики, R остаётся неизменным.

Шаг 4. Обновляем коэффициент Калмана с учетом нового значения R

K = P_pred * H' / (H * P_pred * H' + R_hist(k));
x(:,k) = x_pred + K * innovation;
P(:,:,k) = (eye(2) - K * H) * P_pred;
filtered(k) = x(1,k);

Коэффициент Калмана K теперь использует обновлённое R_hist(k). Чем больше R, тем меньше K, и фильтр больше опирается на модель, а не на измерения. Затем корректируем состояние и ковариацию стандартным способом.

На рисунке показано как R адаптируется к увеличивающемуся уровню шума (напомню, шум мы искусственно увеличили последовательно для каждой из фаз: минимальный при синусе, максимальный на линейном участке).

Мощность заданного нами добавленного шума равна его дисперсии В коде она задаётся переменной noise_power. Обратите внимание, значение R не задавалось нами вручную, но полученное значение (особенно на линейном участке, где шум самый сильный) $R≈\sigma^2.$

Сравнение работы некоторых модификаций алгоритма

Представим визуализацию работы каждого из рассмотренных выше алгоритмов. Чтобы не перегружать статью кодом, сошлюсь на репозиторий.

Наглядно работа каждого алгоритма показана на сводном рисунке ниже.

Как мы обговорили ранее, зашумленность сигнала последовательно увеличивалась для каждого участка. Что мы видим на графиках?

На линейных участках и на участке плато трапеции ускорение равно нулю. Ненулевым и постоянно меняющимся ускорение является только на синусоидальном участке. Именно на этом участке векторный фильтр третьего порядка показывает наилучший результат. На остальных участках его преимущество не так велико. Однако в целом по сигналу он оказывается лучшим (минимальное среднеквадратичное отклонение) благодаря вкладу синусоидальной части.
Результат работы каскадного фильтра визуально получился самым "гладким", платой за это является задержка по времени обработки, что может быть критичным в некоторых системах.
На линейном участке (1,4 — 2 с.) шум самый сильный, адаптивный фильтр демонстрирует на нем лучший результат.

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

Демонстрация работы фильтра на каждом из участков

Выполним настройку параметров

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

Скорректируем параметры векторного фильтра второго и третьего порядка. Уменьшим доверие к измерениям, сделав R=3,5. В векторном фильтре третьего порядка сделаем Q в канале оценки скорости равным 0.02, а в канале ускорения 0.1. У адаптивного фильтра уменьшим в эмпирической формуле коэффициент чувствительности к дисперсии инноваций до 1 и уменьшим начальное доверие к измерениям.

За исключением синусоидального участка, где ускорение не равно нулю, графики работы векторного фильтра второго и третьего порядков практически полностью совпадают.

Итоговая сравнительная диаграмма после настройки

Настройка параметров позволила улучшить работу адаптивного и векторных фильтров. Ошибка работы каскадного фильтра будет всегда выше, это "цена" за гладкость сигнала.

После проведенной настройки сопоставим работу алгоритмов. Сравнительная таблица среднеквадратичного отклонения MSE получившейся оценки каждым методом от истинного значения сигнала представлена ниже.

Метод	MSE за весь период расчета
Скалярный (базовый)	0,258
Скалярный (корректно настроенный)	0,110
Векторный 2 порядка	0,108
Векторный 3 порядка	0,107
Каскадный	0,133
Адаптивный	0,103

Заключение

Полученные значения не являются "истиной в последней инстанции", они характерны для сигнала конкретной формы. Поскольку на трапецеидальном и линейном участках ускорение равно нулю, большого эффекта от работы сложных алгоритмов добиться трудно. Для более хаотичного сигнала этот эффект был бы заметен сильнее. Здесь же целью было показать принцип работы, визуально представить, как работают некоторые модификации алгоритма. Кроме того, для наглядности мы проводим расчет на коротком временном интервале. Если бы мы оценивали ошибку не за 2 с., а, например, за 3600 секунд, разница MSE между модификациями в абсолютном выражении была бы существенная.

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