Собираем простейший фильтр Калмана в Scilab: из зашумлённых измерений — к точной оценке

В этой статье мы соберём простейший фильтр Калмана в Scilab: по сотне зашумлённых
показаний прибора оценим постоянное напряжение источника и на графиках посмотрим,
как оценка сходится к истине. Алгоритм целиком — это пять строк внутри одного цикла,
а всё остальное — данные, графики и проверка результата.

Что будем оценивать

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

Исходные данные:

• истинное значение ;
• число измерений , шаг по времени с;
• дисперсия шума измерений — прибор шумит с разбросом примерно ±1 В;
• дисперсия шума модели — мы допускаем, что значение может чуть «плыть»,
  но почти постоянно.

Шаг 1. Генерируем зашумлённые измерения

Зададим параметры и соберём вектор измерений: истинное значение плюс гауссов шум.
Заодно зафиксируем генератор случайных чисел, чтобы при каждом запуске получать
одни и те же данные и спокойно сравнивать результаты.

clear; clc;

N    = 100;                 // число измерений
dt   = 0.1;                 // шаг времени, с
t    = (0:N-1)' * dt;       // моменты времени

x_true = 5;                 // истинное (неизменное) значение

q    = 0.001;               // дисперсия шума модели
r    = 1;                   // дисперсия шума измерений

rand("seed", 42);
z = x_true + sqrt(r) * rand(N, 1, "normal");  // зашумлённые измерения

Здесь rand(N, 1, "normal") создаёт столбец из N случайных чисел со стандартным
нормальным распределением, а множитель sqrt(r) задаёт нужный разброс. Посмотрим на
сырые данные (красные точки на рисунке 1 ниже): точки разбросаны от 2 до 8 В,
и по отдельности каждое показание мало похоже на 5. Использовать такой поток «как есть»
— всё равно что снимать показания с прибора, у которого дрожит стрелка. Нам нужна
сглаженная, устойчивая оценка — этим и займётся фильтр.

Шаг 2. Модель и формулы фильтра

Фильтр Калмана работает в два такта на каждом шаге: прогноз и коррекция.
Сначала запишем модель. Состояние системы — само значение ; мы считаем его
почти постоянным, поэтому от шага к шагу оно переписывается с малым шумом модели
. Прибор выдаёт измерение с шумом :

Здесь — нормальное распределение с нулевым средним и дисперсией
: шумы не смещают значение, а лишь разбрасывают его.

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

Прогноз. Значение не меняется, поэтому предсказанная оценка (обозначим её
) равна предыдущей, а неуверенность растёт на величину шума модели :

Коррекция. Получив новое измерение , фильтр сдвигает прогноз в его сторону.
Насколько сильно — решает коэффициент усиления Калмана :

Разберём коэффициент . Это отношение нашей неопределённости к суммарной
неопределённости . Если мы совсем не уверены в своей оценке
( велико по сравнению с ), то близок к 1 — фильтр почти полностью
доверяет свежему измерению. Если же оценка уже выверена ( мало), то
близок к 0 — новое показание лишь слегка подправляет результат. Разность
называется невязкой: это расхождение между тем, что показал
прибор, и тем, что мы ожидали увидеть.

Откуда берётся формула для ? Если записать коррекцию с произвольным
коэффициентом и подставить в определение дисперсии ошибки, получится:

Продифференцируем по и приравняем производную нулю:

Решив это уравнение, как раз и получим — то самое
«чудесное» свойство фильтра: оптимальный вес выводится из условия минимальной
ошибки, а не подбирается вручную. Подстановка оптимального обратно даёт
компактное : каждое измерение уменьшает нашу неуверенность,
ведь множитель всегда меньше единицы.

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

Шаг 3. Начальные условия

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

x_hat = zeros(N, 1);        // оценки состояния
P     = zeros(N, 1);        // дисперсия ошибки оценки
K     = zeros(N, 1);        // коэффициент усиления Калмана

x_hat(1) = 0;               // начальное предположение (заметно неверное)
P(1)     = 10;              // большая начальная неопределённость

Шаг 4. Цикл фильтра: прогноз и коррекция

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

for k = 2:N
    // 1. Прогноз (prediction)
    x_pred = x_hat(k-1);        // модель: x(k) = x(k-1)
    P_pred = P(k-1) + q;        // рост неопределённости из-за шума модели

    // 2. Коррекция (update)
    K(k)     = P_pred / (P_pred + r);            // коэффициент усиления
    x_hat(k) = x_pred + K(k) * (z(k) - x_pred);  // новая оценка
    P(k)     = (1 - K(k)) * P_pred;              // новая дисперсия
end
K(1) = P(1) / (P(1) + r);

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

Шаг 5. Запускаем и смотрим числа

В конце скрипта напечатаны итоги:

Начальная оценка: 0.000, конечная оценка: 5.2427 (истинное: 5.0)
СКО шума измерений:              1.0000
СКО ошибки фильтра (установивш.):  0.1768
Коэффициент усиления установился:  K = 0.0312

Стартовав с нуля, фильтр вышел к значению около 5.24 — при истинных 5 это вполне
внутри ожидаемого разброса. Главное — сравните точности: среднеквадратичное отклонение
сырых измерений равно , а установившаяся ошибка фильтра —
. Точность выросла примерно в 5–6 раз: фильтр накопил
статистику и «усреднил» шум умнее, чем это сделало бы простое среднее арифметическое,
потому что ещё и следил за своей неуверенностью.

Шаг 6. Графики и их смысл

Построим три рисунка и сохраним их в PNG функцией xs2png — полный код построения
приведён в конце статьи, а здесь разберём, что видно на каждом.

Красные точки — сырые показания прибора, чёрный пунктир — истинные 5 В,
синяя линия — оценка фильтра. Видно, как оценка за доли секунды поднимается от нуля
к истине, а дальше идёт спокойной линией, не реагируя на каждый отдельный «прыжок»
прибора.

Сверху — : начав с 10, дисперсия ошибки за несколько шагов падает
ниже уровня шума измерений и стремится к установившемуся значению около 0.031.
Снизу — : в первый момент , то есть фильтр почти целиком
доверяет прибору, ведь своей оценки у него ещё нет. По мере накопления данных
убывает до ≈0.03 — фильтр ведёт себя как осторожный инженер: чем увереннее собственная
оценка, тем сдержаннее реакция на каждое новое показание.

Синяя линия — разность оценки и истины, зелёный пунктир — границы
, приблизительно 95% доверительный интервал. Интервал
быстро сжимается, и ошибка остаётся внутри него: фильтр не только даёт оценку, но и
честно сообщает, насколько ей можно доверять.

Что попробовать самостоятельно

• Уменьшите до 0.1 (прибор точнее): коэффициент установится на большем
  уровне, а ошибка фильтра станет ещё меньше.
• Положите : модель станет «абсолютно постоянной», и фильтр превратится в
  накопитель среднего — проверьте, что .
• Задайте или старт : посмотрите, за сколько шагов
  фильтр простит нам ещё более неудачный старт.

Полный код скрипта

Весь листинг kalman_filter.sce целиком — от генерации данных до сохранения графиков:

// Простейший фильтр Калмана: оценка константы по зашумлённым измерениям
clear; clc;

// ---------- Параметры ----------
N    = 100;                 // число измерений
dt   = 0.1;                 // шаг времени, с
t    = (0:N-1)' * dt;       // моменты времени

x_true = 5;                 // истинное (неизменное) значение

q    = 0.001;               // дисперсия шума модели
r    = 1;                   // дисперсия шума измерений

// ---------- Генерация данных ----------
rand("seed", 42);
z = x_true + sqrt(r) * rand(N, 1, "normal");  // зашумлённые измерения

// ---------- Фильтр Калмана ----------
x_hat = zeros(N, 1);        // оценки состояния
P     = zeros(N, 1);        // дисперсия ошибки оценки
K     = zeros(N, 1);        // коэффициент усиления Калмана

// Начальные условия: априорная оценка и её дисперсия
x_hat(1) = 0;               // начальное предположение (заметно неверное)
P(1)     = 10;              // большая начальная неопределённость

for k = 2:N
    // 1. Прогноз (prediction)
    x_pred = x_hat(k-1);        // модель: x(k) = x(k-1)
    P_pred = P(k-1) + q;        // рост неопределённости из-за шума модели

    // 2. Коррекция (update)
    K(k)     = P_pred / (P_pred + r);            // коэффициент усиления
    x_hat(k) = x_pred + K(k) * (z(k) - x_pred);  // новая оценка
    P(k)     = (1 - K(k)) * P_pred;              // новая дисперсия
end
K(1) = P(1) / (P(1) + r);

// ---------- Графики ----------
// Рисунок 1: истинное значение, измерения и оценка фильтра
f1 = scf(1); clf(f1);
f1.figure_size = [900, 550];
plot(t, z, "r.", "markersize", 6);
plot(t, x_true * ones(t), "k--", "thickness", 2);
plot(t, x_hat, "b-", "thickness", 2.5);
legend(["Измерения z(k), дисперсия шума r = " + string(r);
        "Истинное значение x = " + string(x_true);
        "Оценка фильтра Калмана"], 4);
title("Фильтр Калмана: оценка константы по зашумлённым измерениям");
xlabel("Время, с");
ylabel("Измеряемая величина");
gca().box = "on";
xgrid();
xs2png(f1.figure_id, "kalman_measurements.png");

// Рисунок 2: сходимость дисперсии ошибки и коэффициента усиления
f2 = scf(2); clf(f2);
f2.figure_size = [900, 700];

subplot(2, 1, 1);
plot(t, P, "b-", "thickness", 2.5);
plot(t, r * ones(t), "r--", "thickness", 1.5);   // уровень шума измерений
legend(["Дисперсия ошибки оценки P(k)";
        "Дисперсия шума измерений r = " + string(r)], 1);
title("Сходимость дисперсии ошибки оценки");
xlabel("Время, с");
ylabel("P(k)");
gca().box = "on";
xgrid();

subplot(2, 1, 2);
plot(t, K, "r-", "thickness", 2.5);
title("Коэффициент усиления Калмана");
xlabel("Время, с");
ylabel("K(k)");
gca().box = "on";
xgrid();
xs2png(f2.figure_id, "kalman_convergence.png");

// Рисунок 3: ошибка оценки с доверительным интервалом ±2σ
f3 = scf(3); clf(f3);
f3.figure_size = [900, 450];
err = x_hat - x_true;
h_err  = plot(t, err, "b-", "thickness", 2);
h_bnd  = plot(t,  2 * sqrt(P), "g--", "thickness", 1.5);
plot(t, -2 * sqrt(P), "g--", "thickness", 1.5);
plot(t, zeros(t), "k-");
legend([h_err, h_bnd], ["Ошибка оценки (оценка − истина)";
                        "Границы ±2√P(k) (≈95% доверительный интервал)"], 1);
title("Ошибка оценки фильтра Калмана");
xlabel("Время, с");
ylabel("Ошибка");
gca().box = "on";
xgrid();
xs2png(f3.figure_id, "kalman_error.png");

// ---------- Печать результатов ----------
printf("Начальная оценка: %.3f, конечная оценка: %.4f (истинное: %.1f)\n", ..
       x_hat(1), x_hat($), x_true);
printf("СКО шума измерений:              %.4f\n", sqrt(r));
printf("СКО ошибки фильтра (установивш.):  %.4f\n", sqrt(P($)));
printf("Коэффициент усиления установился:  K = %.4f\n", K($));

Итог: мы собрали простейший фильтр Калмана из пяти строк цикла, увидели, как он
сходится от неверного старта к истинному значению, и проверили, что фильтр корректно
оценивает собственную точность. Следующий естественный шаг — добавить в вектор
состояния скорость и оценить уже не константу, а меняющийся сигнал; уравнения останутся
теми же, только запишутся в матричном виде.