\(Kalman\ Filter(卡尔曼滤波器)——Optimal(最优化)\ Recursive(递归)\ Data\ Processing(数据处理)\ Algorithm(算法)\)
\(不确定性:\)
-
不存在完美的数学模型；
-
系统的扰动不可控，也很难建模；
-
测量传感器存在误差。
\(eg:\)
\[\begin{align*}
&z_k 为第k次的测量结果&\\
&\color{red}{估计真实数据\rightarrow 取平均值}\\
\hat{x}_k &= \frac{1}{k} (z_1+z_2+……+z_k)\\
&= \frac{1}{k} (z_1+z_2+……+z_{k-1})+\frac{1}{k} z_k\\
&= \frac{1}{k} \frac{k-1}{k-1} (z_1+z_2+……+z_{k-1})+\frac{1}{k} z_k\\
&= \frac{k-1}{k} \hat{x}_{k-1}+\frac{1}{k} z_k\\
&= \hat{x}_{k-1}+\frac{1}{k} z_k-\frac{1}{k} \hat{x}_{k-1}\\
\color{red}{\Rightarrow \hat{x}_k} &\color{red}{= \hat{x}_{k-1}+\frac{1}{k} (z_k-\hat{x}_{k-1})}\\
&k\uparrow ,\frac{1}{k} (z_k-\hat{x}_{k-1}) \rightarrow 0,\hat{x}_k \rightarrow \hat{x}_{k-1}\\
即:&\\
&随着k的增加，测量结果不再重要;\\
&\Downarrow \frac{1}{k} \rightarrow K_k\\
\color{red}{\Rightarrow \hat{x}_k} &\color{red}{= \hat{x}_{k-1}+K_k (z_k-\hat{x}_{k-1})}\\
&\color{green}{Recursive(递归)}\\
&当前的估计值=上一次的估计值+系数 \times (当前测量值-上一次的估计值)\\
&K_k:Kalman \ Gain(卡尔曼增益/因数)\\
优势:&\\
&卡尔曼滤波器不需要追溯很久以前的数据,只需要知道上一次的就可以了;\\
\end{align*}
\]
\[\begin{align*}
e_{EST}&(估计误差):&\\
&\color{blue}{e \rightarrow Error(误差) \ EST \rightarrow Estimate(估计)}\\
e_{MEA}&(测量误差):\\
&\color{blue}{MEA \rightarrow Measurement(测量)}\\
\color{red}{K_k} &\color{red}{= \frac{e_{{EST}_{k-1}}}{e_{{EST}_{k-1}}+e_{{MEA}_k}}(卡尔曼滤波器的核心公式)}\\
讨论:&\\
在k&时刻:\\
&(1)e_{{EST}_{k-1}} \gg e_{{MEA}_k}:K_k \rightarrow 1,\hat{x}_k = \hat{x}_{k-1}+z_k-\hat{x}_{k-1}=z_k\\
&(1)e_{{EST}_{k-1}} \ll e_{{MEA}_k}:K_k \rightarrow 0,\hat{x}_k = \hat{x}_{k-1}\\
\end{align*}
\]
\[\begin{align*}
Step1:&计算Kalman \ Gain,K_k= \frac{e_{{EST}_{k-1}}}{e_{{EST}_{k-1}}+e_{{MEA}_k}}\\
Step2:&计算\hat{x}_k= \hat{x}_{k-1}+K_k (z_k-\hat{x}_{k-1})\\
Step3:&更新e_{{EST}_k}=(I-K_k)e_{{EST}_{k-1}}&
\end{align*}
\]
\(eg:\)
\[\begin{align*}
&实际长度x=50mm,\hat{x}_0 =40mm,e_{{EST}_0}=5mm,z_1=51mm,e_{{MEA}_k}=3mm.&\\
\end{align*}
\]
| \(k\) | \(z_k\) | \(e_{{MEA}_k}\) | \(\hat{x}_k\) | \(K_k\) | \(e_{{EST}_k}\) |
|---|---|---|---|---|---|
| \(0\) | \({\color{brown}{}}\) | \({\color{red}{3}}\) | \({\color{gray}{40}}\) | \({\color{green}{}}\) | \({\color{red}{5}}\) |
| \(1\) | \({\color{brown}{51}}\) | \({\color{red}{3}}\) | \({\color{gray}{46.875}}\) | \({\color{green}{0.625}}\) | \({\color{red}{1.875}}\) |
| \(2\) | \({\color{brown}{48}}\) | \({\color{red}{3}}\) | \({\color{gray}{47.308}}\) | \({\color{green}{0.3846}}\) | \({\color{red}{1.154}}\) |
| \(3\) | \({\color{brown}{47}}\) | \({\color{red}{3}}\) | \({\color{gray}{47.222}}\) | \({\color{green}{0.2778}}\) | \({\color{red}{0.833}}\) |
| \(4\) | \({\color{brown}{52}}\) | \({\color{red}{3}}\) | \({\color{gray}{48.261}}\) | \({\color{green}{0.2174}}\) | \({\color{red}{0.652}}\) |
| \(5\) | \({\color{brown}{51}}\) | \({\color{red}{3}}\) | \({\color{gray}{48.750}}\) | \({\color{green}{0.1786}}\) | \({\color{red}{0.536}}\) |
| \(6\) | \({\color{brown}{48}}\) | \({\color{red}{3}}\) | \({\color{gray}{48.636}}\) | \({\color{green}{0.1515}}\) | \({\color{red}{0.455}}\) |
| \(7\) | \({\color{brown}{49}}\) | \({\color{red}{3}}\) | \({\color{gray}{48.684}}\) | \({\color{green}{0.1316}}\) | \({\color{red}{0.395}}\) |
| \(8\) | \({\color{brown}{53}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.186}}\) | \({\color{green}{0.1163}}\) | \({\color{red}{0.349}}\) |
| \(9\) | \({\color{brown}{48}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.063}}\) | \({\color{green}{0.1042}}\) | \({\color{red}{}}\)\({\color{red}{0.313}}\) |
| \(10\) | \({\color{brown}{49}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.057}}\) | \({\color{green}{0.0943}}\) | \({\color{red}{0.283}}\) |
| \(11\) | \({\color{brown}{52}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.310}}\) | \({\color{green}{0.0862}}\) | \({\color{red}{0.259}}\) |
| \(12\) | \({\color{brown}{53}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.603}}\) | \({\color{green}{0.0794}}\) | \({\color{red}{0.238}}\) |
| \(13\) | \({\color{brown}{51}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.706}}\) | \({\color{green}{0.0735}}\) | \({\color{red}{0.221}}\) |
| \(14\) | \({\color{brown}{52}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.863}}\) | \({\color{green}{0.0685}}\) | \({\color{red}{0.205}}\) |
| \(15\) | \({\color{brown}{49}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.808}}\) | \({\color{green}{0.0641}}\) | \({\color{red}{0.192}}\) |
| \(16\) | \({\color{brown}{50}}\) | \({\color{red}{3}}\) | \({\color{gray}{49.819}}\) | \({\color{green}{0.0602}}\) | \({\color{red}{0.181}}\) |
\[\begin{align*}
k=1:&\\
K_1&={\color{red}{\frac{5}{3+5}}}= {\color{green}{0.625}},&\\
\hat{x}_1&={\color{gray}{40}}+{\color{green}{0.625}} \cdot({\color{brown}{51}}-{\color{gray}{40}})={\color{gray}{46.875}},\\
e_{{EST}_1}&=(1-{\color{green}{0.625}})\cdot {\color{red}{5}}={\color{red}{1.875}};\\
k=2:&\\
K_2&={\color{red}{\frac{1.875}{3+1.875}}}={\color{green}{0.3846}},\\
\hat{x}_2&={\color{gray}{46.875}}+{\color{green}{0.3846}} \cdot ({\color{brown}{48}}-{\color{gray}{46.875}})={\color{gray}{47.308}},\\
e_{{EST}_2}&=(1-{\color{green}{0.3846}}) \cdot {\color{red}{1.875}}={\color{red}{1.154}};\\
&……
\end{align*}
\]
