单页网站开发,移动网站怎么做,云南城乡建设网站,建设网站网络公司原理简述
卡尔曼滤波可以在线性模型#xff0c;误差为高斯模型的情况下#xff0c;对目标状态得出很好的估计效果#xff0c;但如果系统存在非线性的因素#xff0c;其效果就没有那么好了。比较典型的非线性函数关系包括平方关系#xff0c;对数关系#xff0c;指数关系…原理简述
卡尔曼滤波可以在线性模型误差为高斯模型的情况下对目标状态得出很好的估计效果但如果系统存在非线性的因素其效果就没有那么好了。比较典型的非线性函数关系包括平方关系对数关系指数关系三角函数关系等。
对于非线性系统的问题比较常用的方法是将非线性的部分进行线性化处理其中EKF 就是对非线性系统进行线性化使用泰勒展开式略去二阶及以上项(数据较小)得到一个近似的线性化模型然后应用卡尔曼滤波完成估计。
EKF与KF的对比推导
卡尔曼滤波的五个公式 泰勒展开式为 在 EKF 中泰勒展开式只保留前两项即
f(x) ≈ f(x0) f(x0)*(x - x0)
在进行展开时将估计值作为 x0 因为估计值是确定的值实际状态 x 作为 x来展开
对泰勒展开式来说f(x0) 相当于是常数项。
估计方程与量测方程
线性卡尔曼滤波的离散估计状态方程与量测方程为 其中 z(k) 为量测值H 为量测值与状态 x 之间的关系矩阵。
在 EKF 中将等式右侧的项都看作是一个非线性函数也就是状态方程中将等式右边的关于x_hat 与 u 关系式都记作 f量测方程中等式右边的函数关系记作是 h 即 其中f 是非线性状态函数h 是非线性量测函数w 与 v 分别是高斯噪声协方差矩阵为 Q 与 R在上式中省略了。
在 EKF 的求解中将 f 与 h 进行泰勒展开f(x0)与h(x0) 当作是常数项x0 代入 x_hat然后只保留前两项(在 x_hat(k) 处进行展开)将一阶偏导数分别记作 F 与 H后续的卡尔曼公式会用到 F 与 H其实 F 与 H 都是雅可比矩阵 将偏导部分记作 F 量测方程的非线性函数展开为 将偏导部分记作 H 结合上面的非线性方程之后将线性化的泰勒展开式代入上面的非线性方程。
状态估计方程 即将非线性函数在上次估计值 x_hat(k-1) 处进行泰勒展开其中f(x_hat(k-1)) 就是上一次的先验估计值与 F*x_hat(k-1) 都是非随机值将两个非随机值提出得到 量测方程 x_mea 为量测状态同样的将非随机值提出得到 协方差矩阵与卡尔曼增益
根据先验估计的表达式由 F 来代替可以得出先验误差的协方差公式 卡尔曼增益为 后验估计与协方差矩阵更新
后验估计与协方差矩阵都与KF是相同的。
后验估计 先验估计值 卡尔曼增益*(量测值 - 先验估计输出) 更新协方差 EKF的更新公式
预测
先验估计值计算 先验误差协方差计算 校正
卡尔曼增益计算 后验估计值计算 更新估计误差协方差 一维非线性例子
这里直接采用了一本书上的比较典型的非线性例子假设系统方程包含非线性项
x(k) 0.5*x(k-1) 2.5*x(k-1) / (1 x(k-1)^2) 8*cos(1.2k) w(k)
其中 w 为系统过程噪声。
观测方程为
z(k) x(k)^2 / 20 v(k)
其中 v 为测量噪声
在上述例子中系统方程与观测方程都包含非线性部分在求解时可以按照如下步骤
1状态预测计算先验估计值
x(k)- 0.5*x(k-1) 2.5*x(k-1) / (1 x(k-1)^2) 8*cos(1.2k)
2将上一步计算的先验估计值代入F与H更新线性化方程的偏导雅可比矩阵分式求导法则 (u/v) (uv-uv)/v²
F 0.5 (2.5 * (1x^2) - 2.5*x*2*x) / (1x^2)^2 0.5 2.5*(1-x^2) / (1x^2)^2
H x/10
3求协方差矩阵的预测矩阵 Pk- 4求卡尔曼滤波增益K 5更新状态 例如xt 为上时刻的估计状态x为当前的先验估计值则
h(x) h(xt) H*(x - xt) 6更新协方差 Pk 在实际测试中将量测值加入一定范围的噪声最后的效果如图
