Обозначим за \(x_{t}\) величину, которую мы будем измерять, а потом фильтровать. Мы будем измерять координату падающего камня. Движение камня задано формулой: \[x_{t}=400\cdot t-4.9\cdot t^2\] Выразим координату камня через ускорение и предыдущую позицию камня: \[x_{t+1}=400\cdot (t+1)-4.9\cdot (t+1)^2=400\cdot t+400-4.9\cdot t^2-9.8\cdot t-4.9=x_{t}-9.8\cdot t+395.1\] Так как взяв два раза производную от \(x_{t}\) получим, что ускорение равно -9.8, то координата камня будет изменяться по закону: \[x_{t+1}=x_{t}+a\cdot t+395.1\] где \(a=-9.8\) Но в реальной жизни мы не можем учесть в наших расчетах маленькие возмущения, действующие на камень, такие как: ветер, сопротивление воздуха и т.п., поэтому настоящая координата камня будет отличаться от расчетной. К правой части написанного уравнения добавится случайная величина \(E_{t}\) \[x_{t+1}=x_{t}+a\cdot t+395.1+E_{t}\] Мы установили на земле под камнем дальномер. Дальномер будет измерять координату \(x_{t}\), но, к сожалению, он не может точно измерить ее и мерит с ошибкой \(N_{t}\),которая тоже является случайной величиной: \[z_{t}=x_{t}+N_{t}\] Задача состоит в том, чтобы, зная неверные показания дальномера \(z_{t}\), найти хорошее приближение для истинной координаты камня \(x_{t}\). Это приближение мы будем обозначать \(x_{t}^{opt}\). Таким образом, уравнение для координаты и показания дальномера будут выглядеть следующим образом: \[\begin{cases} x_{t+1}=x_{t}+a\cdot t+395.1+E_{t}\\ z_{t}=x_{t}+N_{t} \end{cases}\]
Идея Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате \(x_{t+1}\),мы должны выбрать золотую середину между показанием \(z_{t+1}\) неточного сенсора и \(x^{opt}_{t}+a\cdot t+395.1\) — нашим предсказанием того, что мы ожидали от него увидеть. Показанию сенсора мы дадим вес \(K\), а на предсказанное значение останется вес \((1-K)\): \[x^{opt}_{t+1}=K_{t+1}\cdot z_{t+1}+(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+395.1)\] где \(K_{t+1}\) - коэффициент Калмана, зависящий от шага итерации.
Мы должны выбрать \(K_{t+1}\) таким, чтобы получившееся оптимальное значение координаты \(x^{opt}_{t+1}\) было бы наиболее близкое к истиной координате \(x_{t+1}\). В общем случае, чтобы найти точное значение коэффициента Калмана \(K_{t+1}\) , нужно просто минимизировать ошибку: \[e_{t+1}=x_{t+1}-x^{opt}_{t+1}\] Подставляем в уравнение (7) выражение (8) и упрощаем: \[e_{t+1}=x_{t+1}–K_{t+1}\cdot z_{t+1}-(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+404.9)=\\=x_{t+1}-K_{t+1}\cdot (x_{t+1}+N_{t+1})-(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+404.9)=\\=x_{t+1}\cdot (1-K_{t+1})-K_{t+1}\cdot (x_{t+1}+N_{t+1})-(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+404.9)=\\=(1-K_{t+1})\cdot (x_{t+1}–x^{opt}_{t}–a\cdot t–404.9)–K_{t+1}\cdot N_{t+1}=\\=(1-K_{t+1})\cdot (x_{t}+a\cdot t+404.9+E_{t}–x^{opt}_{t}–a\cdot t–404.9)–K_{t+1}\cdot N_{t+1}=\\=(1-K_{t+1})\cdot (x_{t}–x^{opt}_{t}+E_{t})–K_{t+1}\cdot N_{t+1}=\\=(1-K_{t+1})\cdot (e_{t}+E_{t})–K_{t+1}\cdot N_{t+1}\] Таким образом, получаем: \[e_{t+1}=(1-K_{t+1})\cdot (e_{t}+E_{t})–K_{t+1}\cdot N_{t+1}\] Мы будем минимизировать среднее значение от квадрата ошибки: \(E(e^{2}_{t+1})\longrightarrow\min\) Т.к. все входящие в \(e_{t+1}\)) случайные величины независимые и средние значения ошибок сенсора и модели равны нулю: \(E[E_{t}]=E[N_{t+1}]=0\), и все перекрестные значения равны нулю: \(E[E_{t}\cdot N_{t+1}]=E[e_{t}\cdot E_{t}]=E[e_{t}\cdot N_{t+1}]=0\), то получаем: \[E(e^{2}_{t+1})=(1-K_{t+1})^{2}\cdot (E(e^{2}_{t})+D(E_{t}))+K^{2}_{t+1}\cdot D(N_{t})\] Где \(D(E_{t})\) и \(D(N_{t+1})\)-дисперсии случайных величин \(E_{t}\) и \(N_{t+1}\). Найдем минимальное значение для выражения (11) (т.е. найдем производную): \[-2\cdot (1-K_{t+1})\cdot (E(e^{2}_{t})+D(E_{t}))+2\cdot K_{t+1}\cdot D(N_{t})=0\] \[-E(e^{2}_{t})–D(E_{t})+K_{t+1}\cdot E(e^{2}_{t})+K_{t+1}\cdot D(E_{t})+K_{t+1}\cdot D(N_{t})=0\] \[K_{t+1}=\frac{E(e^{2}_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}\] Таким образом, получаем такое \(K_{t+1}\), что выражение \(E(e^{2}_{t+1})\) будет минимальным: \[K_{t+1}=\frac{E(e^{2}_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}\] Случайные величины имеют нормальный закон распределения и мы знаем, что их дисперсии равны: \(\delta^{2}_{E}\) и \(\delta^{2}_{N}\). Заметим, что дисперсии не зависят от t, потому что законы распределения не зависят от него. Подставляем в выражение для среднеквадратичной ошибки \(E(e^{2}_{t+1})\) минимизирующее ее значение коэффициента Калмана \(K_{t+1}\) и получаем: \[E(e^{2}_{t+1})=(1-\frac{E(e^{2}_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot (E(e^{2}_{t})+\delta^{2}_{E})+(\frac{E(e^{2}_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot \delta^{2}_{N}\] \[E(e^{2}_{t+1})=(1-\frac{E(e^{2}_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot (E(e^{2}_{t})+\delta^{2}_{E})+(\frac{E(e^{2}_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot \delta^{2}_{N}=\frac{(\delta^{2}_{N})^{2}\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N})^{2}}+\frac{\delta^{2}_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})^{2}}{(E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N})^{2}}=\\=\frac{\delta^{2}_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})\cdot (E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N})}{(E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N})^{2}}=\frac{c\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}}=\frac{\delta^{2}_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}}\] Таким образом, получаем: \[E(e^{2}_{t+1})=\frac{\delta^{2}_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}} - среднее\ значение\ квадрата\ ошибки\] Таким образом, мы получили формулу для вычисления коэффициента Калмана.
Теперь, когда мы познакомились с фильтром Калмана, реализуем пример на Delphi. Для тестирования программы мы воспользуемся Delphi 7
Код:
unit Unit1;
interface
uses
Windows, Messages, SysUtils, Variants, Classes, Graphics, Controls, Forms,
Dialogs, StdCtrls, ExtCtrls;
type
TForm1 = class(TForm)
img1: TImage;
edt1: TEdit;
edt2: TEdit;
lbl1: TLabel;
lbl2: TLabel;
edt3: TEdit;
lbl3: TLabel;
edt4: TEdit;
lbl4: TLabel;
btn1: TButton;
procedure btn1Click(Sender: TObject);
private
{ Private declarations }
public
{ Public declarations }
end;
var
Form1: TForm1;
implementation
{$R *.dfm}
procedure TForm1.btn1Click(Sender: TObject);
var
N, i: Integer;
a, sigma_psi, sigma_eta: Real;
x, z, xOpt, eOpt, k: array [1..100] of Real;
begin
Randomize;
N:=StrToInt(edt1.Text);
a:=StrToFloat(edt2.Text);
sigma_psi:=Sqrt(StrToFloat(edt3.Text));
sigma_eta:=Sqrt(StrToFloat(edt4.Text));
x[1]:=0;
z[1]:=x[1]+Random(Trunc(sigma_eta))-sigma_eta/2;
for i:=1 to N-1 do
begin
x[i+1]:=x[i]+395.1+a*i+random(Trunc(sigma_psi))-sigma_psi/2;
z[i+1]:=x[i+1]+random(Trunc(sigma_eta))-sigma_eta/2;
if x[i+1]<0 then x[i+1]:=0;
if z[i+1]<0 then z[i+1]:=0;
end;
xOpt[1]:=1;
eOpt[1]:=sigma_eta;
for i:=1 to N-1 do
begin
eOpt[i+1]:=sqrt((Sqr(sigma_eta))*(Sqr(eOpt[i])+sqr(sigma_psi))/(sqr(sigma_eta)+sqr(eOpt[i])+sqr(sigma_psi)));
k[i+1]:=(Sqr(eOpt[i+1]))/sqr(sigma_eta);
xOpt[i+1]:=(xOpt[i]+a*i)*(1-k[i+1])+k[i+1]*z[i+1];
if xOpt[i+1]<0 then xOpt[i+1]:=0;
end;
img1.Picture:=nil;
img1.Canvas.MoveTo(30,40);
img1.Canvas.LineTo(30,450);
img1.Canvas.LineTo(540,450);
img1.Canvas.Pen.Color:=clLime;
img1.Canvas.MoveTo(30,450-round(xOpt[1]));
for i:=2 to N do
begin
img1.Canvas.LineTo(30+(i-1)*5,450-round(xOpt[i]*0.05));
end;
img1.Canvas.Pen.Color:=clRed;
img1.Canvas.MoveTo(30,450-round(z[1]));
for i:=2 to N do
begin
img1.Canvas.LineTo(30+(i-1)*5,450-round(z[i]*0.05));
end;
img1.Canvas.Pen.Color:=clBlue;
img1.Canvas.MoveTo(30,450-round(x[1]));
for i:=2 to N do
begin
img1.Canvas.LineTo(30+(i-1)*5,450-round(x[i]*0.05));
end;
Img1.Canvas.Textout(15, 455, '0');
Img1.Canvas.Textout(10, 390, '1200');
Img1.Canvas.Textout(10, 330, '2400');
Img1.Canvas.Textout(10, 270, '3600');
Img1.Canvas.Textout(10, 210, '4800');
Img1.Canvas.Textout(5, 150, '6000');
Img1.Canvas.Textout(5, 90, '7200');
Img1.Canvas.Textout(5, 30, '8400');
Img1.Canvas.Textout(30, 20, 'V');
Img1.Canvas.Textout(130, 455, '20');
Img1.Canvas.Textout(230, 455, '40');
Img1.Canvas.Textout(330, 455, '60');
Img1.Canvas.Textout(430, 455, '80');
Img1.Canvas.Textout(530, 455, '100');
Img1.Canvas.Textout(550, 445, 't');
end;
end.
Запустим программу и получим график на котором будут располагаться 3 линии: линия истинного положения камня, линия показания сенсора и линия полученная после использования фильтра Калмана.
Результат выполнения программы: