ROUGH DRAFT authorea.com/114940
Main Data History
Export
Show Index Toggle 5 comments
  •  Quick Edit
  • Алгоритмы локации и маршрутизации. Алгоритм Калмана-Мельников

    Постановка задачи

    Обозначим за \(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. Для тестирования программы мы воспользуемся 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 линии: линия истинного положения камня, линия показания сенсора и линия полученная после использования фильтра Калмана.
    Результат выполнения программы: