- 前言
- 系统建模
- 贝叶斯滤波
- 先验
- 似然
- 后验
控制算法学习一(1)是以小车为例，推演卡尔曼滤波算法的流程。
举例易于理解算法，但如果要真正掌握算法，必须从算法的本质入手进行推导。
卡尔曼滤波的本质，就是贝叶斯滤波在高斯过程上处理的特例。
系统建模仍然以小车模型为例：
x k = A x k − 1 + B u k − 1 + w k − 1 , w k ∼ [ 0 , Q ] z k = H x k + v k , v k ∼ [ 0 , R ] \begin{aligned} & x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}, w_k\sim [0, Q] \\ & z_k = Hx_k+v_{k} ,v_k\sim[0, R] \end{aligned} xk=Axk−1+Buk−1+wk−1,wk∼[0,Q]zk=Hxk+vk,vk∼[0,R] A , B , H A,B,H A,B,H分别表示状态转移矩阵，控制（输入）转换矩阵，观测转换矩阵。
w k , v k w_k,v_k wk,vk都是高斯白噪声，表示了系统误差（过程噪声）和测量误差。
x k , z k x_k,z_k xk,zk是向量，表明k时刻的系统状态。
小车系统是具有马尔可夫性的，并假设它的位置和速度服从高斯分布，其有噪状态观测 Z = { z 1 , z 2 , … , z n } Z=\{ z_1,\ z_2, \ \dots, z_n \} Z={z1, z2, …,zn}，本时刻的观测是 z n z_n zn，卡尔曼滤波的目标是获得本时刻系统真实状态的最优估计 x n x_n xn。
重点：系统具有马尔科夫性，并且系统状态服从高斯分布
贝叶斯滤波由贝叶斯滤波（3）得到： p ( x n ∣ x n − 1 , u n , z n ) = p ( z n ∣ x n , x n − 1 , u n ) p ( x n ∣ x n − 1 , u n ) p ( z n ∣ x n − 1 , u n ) = p ( z n ∣ x n ) p ( x n ∣ x n − 1 , u n ) p ( z n ∣ x n − 1 , u n ) = p ( z n ∣ x n ) p ( x n ∣ x n − 1 ) p ( z n ) \begin{aligned} p(x_n|x_{n-1},u_n,z_n) & = \frac {p(z_n|x_n,x_{n-1},u_n)p(x_n|x_{n-1},u_n)} {p(z_n|x_{n-1},u_n)} \\ &= \frac {p(z_n|x_n)p(x_n|x_{n-1},u_n)} {p(z_n|x_{n-1},u_n)} \\ &= \frac {p(z_n|x_n)p(x_n|x_{n-1})} {p(z_n)} \\ \end{aligned} p(xn∣xn−1,un,zn)=p(zn∣xn−1,un)p(zn∣xn,xn−1,un)p(xn∣xn−1,un)=p(zn∣xn−1,un)p(zn∣xn)p(xn∣xn−1,un)=p(zn)p(zn∣xn)p(xn∣xn−1) 控制输入量 u n u_n un并不本质，推导省略控制输入
先验上一时刻的状态最优估计是当前状态的先验，根据高斯分布的性质计算 p ( x n ∣ x n − 1 ) p(x_n|x_{n-1}) p(xn∣xn−1)： x n = A x n − 1 + w n − 1 , w n ∼ [ 0 , Q ] x ~ n ∼ N [ A x n − 1 , A P n − 1 A T + Q ] x_n=Ax_{n-1} + w_{n-1}, w_n\sim [0,Q] \\ \tilde x_{n} \sim N[Ax_{n-1}, AP_{n-1}A^T+Q] xn=Axn−1+wn−1,wn∼[0,Q]x~n∼N[Axn−1,APn−1AT+Q] 这里的 x n − 1 x_{n-1} xn−1即上一时刻系统状态的最优估计。 x ~ n \tilde x_{n} x~n即贝叶斯思想里的先验。
似然观测得到的状态是真实状态的似然 p ( z n ∣ x n ) p(z_n|x_n) p(zn∣xn)： z n = H x n + v n , v n ∼ N [ 0 , R ] z_n = Hx_n+v_{n} ,v_n\sim N[0, R] zn=Hxn+vn,vn∼N[0,R]
后验后验 ∝ \propto ∝先验 × \times ×似然
p ( x n ∣ x n − 1 , z n ) = p ( z n ∣ x n ) p ( x n ∣ x n − 1 ) p ( z n ) p(x_n|x_{n-1},z_n) = \frac {p(z_n|x_n)p(x_n|x_{n-1})} {p(z_n)} \\ p(xn∣xn−1,zn)=p(zn)p(zn∣xn)p(xn∣xn−1) 先验、似然均已知，通过卡尔曼的预测和更新过程的公式就可以把 p ( x n ∣ x n − 1 , z n ) p(x_n|x_{n-1},z_n) p(xn∣xn−1,zn)的具体表达式求出。 p ( x n ∣ x n − 1 , z n ) p(x_n|x_{n-1},z_n) p(xn∣xn−1,zn)的期望和协方差就作为当前时刻的状态最优估计。
