描述:卡尔曼滤波是一种优化估计算法(an optimal estimation algorithm)
卡尔曼滤波器也称传感器融合算法
卡尔曼滤波器最早应用在阿波罗登月工程中,用来估算载人航天往返月球的轨迹
— — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — ————————————————————————————————————————————— —————————————————————————————————————————————
如果你想检测火箭燃烧室的温度,无法直接在燃烧室放传感器,只能将传感器放在燃烧室外部
这时 Kalman filter可以作为状态观测器,输入是燃烧室外某个位置的温度,输出是燃烧室内部的温度。
假如你开着一辆车,车上有三种传感器:
我们可以构造卡尔曼滤波器,输入时三个传感器的度数,输出是小车的估算位置。
状态观测器用来帮助你估计无法直接查看或测量的内容。
在数学中,常用$\hat{x} $来表示估计状态。
目标: x ^ 逼 近 于 x \hat{x}逼近于x x^逼近于x
假设一个线性化系统解释一波:
e o b s = x − x ^ x ˙ = A x + B u ① x ^ ˙ = A x ^ + B u + K ( y − y ^ ) ② y = C x , y ^ = C x ^ ① − ② = e o b s ˙ = ( A − K C ) e o b s ⇒ e o b s ( t ) = e ( A − K C ) t e o b s ( 0 ) 我 们 希 望 ( A − K C ) < 0 e_{obs} = x - \hat{x} \\ \dot{x}=Ax+Bu①\\ \dot{\hat{x}}=A\hat{x}+Bu+K(y-\hat{y}) ②\quad y = Cx,\hat{y}=C\hat{x} \\ ①-②=\dot{e_{obs}}=(A-KC)e_{obs} \Rightarrow e_{obs}(t)=e^{(A-KC)t}e_{obs}(0) \\ 我们希望(A-KC)< 0 eobs=x−x^x˙=Ax+Bu①x^˙=Ax^+Bu+K(y−y^)②y=Cx,y^=Cx^①−②=eobs˙=(A−KC)eobs⇒eobs(t)=e(A−KC)teobs(0)我们希望(A−KC)<0
u k : 过 程 噪 声 , 可 能 是 风 速 , 轮 胎 抖 动 造 成 的 v k : 测 量 噪 声 u_k:过程噪声,可能是风速,轮胎抖动造成的 \\ v_k:测量噪声 \qquad \qquad \qquad \quad \quad \quad \quad \quad \quad uk:过程噪声,可能是风速,轮胎抖动造成的vk:测量噪声
通常情况下,这些噪声是服从某一个特定分布的,比如:
测量值并不能反映真实状态且我们也不发简历完全准确的数学模型来反映y与u的关系
已知: \quad ( u k , y k − 1 ) 和 c a r − m o d e l (u_{k},y_{k-1})和car-model (uk,yk−1)和car−model
根据已知推理得到的 x k − 1 ^ \hat{x_{k-1}} xk−1^本身就有误差(噪声),再根据 x k − 1 ^ \hat{x_{k-1}} xk−1^ 以及已知推理得到的 x k ^ \hat{x_{k}} xk^的误差更大
之后经过一个时间单位,我们测得 y k y_k yk,然后根据 y k 和 x k ^ ⇒ x k y_k和\hat{x_k} \Rightarrow x_{k} yk和xk^⇒xk
卡 尔 曼 滤 波 器 就 是 一 种 为 随 机 系 统 设 计 的 状 态 观 测 器 x k ^ = A x k − 1 ^ + B u k + K ( y k − C ( A x k − 1 ^ + B u k ) ) {\color{Red}卡尔曼滤波器就是一种为随机系统设计的状态观测器 \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad} \\ {\color{Red} \hat{x_k}=A\hat{x_{k-1}}+Bu_k+K(y_k-C(A\hat{x_{k-1}}+Bu_k))} 卡尔曼滤波器就是一种为随机系统设计的状态观测器xk^=Axk−1^+Buk+K(yk−C(Axk−1^+Buk))
因此我们发现,卡尔曼滤波器分两步:
P k − P_k^- Pk−用来描述噪声导致的不确定性的影响,Q代表过程噪声的协方差
R代表测量噪声的协方差
调整K使得 P k P_k Pk最小, 即通过调整K,对于数学模型预测出来的值和状态观测反推出来的值权重加和
即如果噪声小,那么后验估值更可信,如果噪声大,预测估值更可信
我们可以假设这样两个极端情况;
P k − = 0 ( 过 程 噪 声 协 方 差 为 0 ) ⇒ K = A P k − 1 A T C T − − − − = 0 ⇒ x k ^ = x k − ^ P_{k}^-=0(过程噪声协方差为0) \Rightarrow K=\frac{AP_{k-1}A^TC^T}{----}=0 \Rightarrow \hat{x_k}=\hat{x_k^-} Pk−=0(过程噪声协方差为0)⇒K=−−−−APk−1ATCT=0⇒xk^=xk−^
系 统 的 状 态 方 程 变 为 : x k = f ( x k − 1 , u k ) + w k y k = g ( x k ) + v k 系统的状态方程变为: \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad\qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad \qquad \\ x_k = f(x_{k-1},u_k)+w_k \\ y_k = g(x_{k})+v_k 系统的状态方程变为:xk=f(xk−1,uk)+wkyk=g(xk)+vk
因为卡尔曼滤波器只针对线性系统,此处必须使用非线性状态估算器来代替卡尔曼滤波器。
我们称之为Extended Kalman Filter(扩展卡尔曼滤波器)
但是EKF有很多缺点,一次线性化并不能完美的表示抖动比较剧烈的系统
在原始概率分布均匀选择采样点,然后映射到非线性变换之后,按照采样点映射后的值重新构造一个最近似的高斯分布
就是把UKF中的采样点换成大量的随机分布的采样点,然后直接得到变换的概率模型
我们可以用旋转电位计测得单摆线与垂直方向的夹角 θ \theta θ, 但是我们的测量结果有很多噪声
step 1 建立数学模型
step 2 simulink仿真
下载simscape_multibody创建的单摆模行
打开文件夹内的 projrct.prj --> 打开virtualPendulumModel.slx(允许高版本模型)
English | Chinese |
---|---|
Pivot | 天花板顶座 |
Revolute Joint | 转轴 |
Link | 连杆 |
KF通过最小化误差协方 P k P_k Pk来求解卡尔曼增益 K K K,最终求解出 x ^ \hat{x} x^
选择 θ 0 = π 18 \theta_0 =\frac{\pi}{18} θ0=18π
KF适用于线性系统,但是单摆模行不是一个线性系统,选择小范围,非线性失真小一些
KF的观测效果良好
选择 θ 0 = π 3 \theta_0 =\frac{\pi}{3} θ0=3π
但是开摆范围较大的时候,KF的效果很差,这时需要引入非线性模型EKF
扩展卡尔曼滤波器估算的离散非线性系统的状态。
显然,相比于KF,EKF这种非线性模型对于非线性系统有很好的效果。