ПОСТАНОВКА ЗАДАЧИ Обозначим за xt величину, которую мы будем измерять, а потом фильтровать. Мы будем измерять координату падающего камня. Движение камня задано формулой: $$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$$ Так как взяв два раза производную от xt получим, что ускорение равно -9.8, то координата камня будет изменяться по закону: $$x_{t+1}=x_{t}+a\cdot t+395.1$$ где a = −9.8 Но в реальной жизни мы не можем учесть в наших расчетах маленькие возмущения, действующие на камень, такие как: ветер, сопротивление воздуха и т.п., поэтому настоящая координата камня будет отличаться от расчетной. К правой части написанного уравнения добавится случайная величина Et $$x_{t+1}=x_{t}+a\cdot t+395.1+E_{t}$$ Мы установили на земле под камнем дальномер. Дальномер будет измерять координату xt, но, к сожалению, он не может точно измерить ее и мерит с ошибкой Nt,которая тоже является случайной величиной: $$z_{t}=x_{t}+N_{t}$$ Задача состоит в том, чтобы, зная неверные показания дальномера zt, найти хорошее приближение для истинной координаты камня xt. Это приближение мы будем обозначать xtopt. Таким образом, уравнение для координаты и показания дальномера будут выглядеть следующим образом: $$ x_{t+1}=x_{t}+a\cdot t+395.1+E_{t}\\ z_{t}=x_{t}+N_{t} $$ АЛГОРИТМ КАЛМАНА Идея Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате xt + 1,мы должны выбрать золотую середину между показанием zt + 1 неточного сенсора и xtopt + a ⋅ 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)$$ где Kt + 1 - коэффициент Калмана, зависящий от шага итерации. Мы должны выбрать Kt + 1 таким, чтобы получившееся оптимальное значение координаты xt + 1opt было бы наиболее близкое к истиной координате xt + 1. В общем случае, чтобы найти точное значение коэффициента Калмана Kt + 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(et + 1²)→min Т.к. все входящие в et + 1) случайные величины независимые и средние значения ошибок сенсора и модели равны нулю: E[Et]=E[Nt + 1]=0, и все перекрестные значения равны нулю: E[Et ⋅ Nt + 1]=E[et ⋅ Et]=E[et ⋅ Nt + 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(Et) и D(Nt + 1)-дисперсии случайных величин Et и Nt + 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}=_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}$$ Таким образом, получаем такое Kt + 1, что выражение E(et + 1²) будет минимальным: $$K_{t+1}=_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}$$ Случайные величины имеют нормальный закон распределения и мы знаем, что их дисперсии равны: δE² и δN². Заметим, что дисперсии не зависят от t, потому что законы распределения не зависят от него. Подставляем в выражение для среднеквадратичной ошибки E(et + 1²) минимизирующее ее значение коэффициента Калмана Kt + 1 и получаем: $$ E(e^{2}_{t+1})=(1-_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot (E(e^{2}_{t})+\delta^{2}_{E})+(_{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-_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot (E(e^{2}_{t})+\delta^{2}_{E})+(_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot \delta^{2}_{N}=_{N})^{2}\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N})^{2}}+_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})^{2}}{(E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N})^{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}}=_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}}=_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}} $$ Таким образом, получаем: $$ E(e^{2}_{t+1})=_{N}\cdot (E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}} - среднее\ значение\ квадрата\ ошибки $$ Таким образом, мы получили формулу для вычисления коэффициента Калмана. DELPHI Теперь, когда мы познакомились с фильтром Калмана, реализуем пример на 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 линии: линия истинного положения камня, линия показания сенсора и линия полученная после использования фильтра Калмана. Результат выполнения программы: