苏州专业做网站的公司有哪些,网络机柜定制,软件开发文档写作,网站购物车功能讲解关于slam一系列文章汇总链接:史上最全slam从零开始#xff0c;针对于本栏目讲解的 卡尔曼家族从零解剖 链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解#xff1a;https://blog.csdn.net/weixin_43013761/article/details/133846882 文末正下方中心提供了本人 联系…讲解关于slam一系列文章汇总链接:史上最全slam从零开始针对于本栏目讲解的 卡尔曼家族从零解剖 链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解https://blog.csdn.net/weixin_43013761/article/details/133846882 文末正下方中心提供了本人 联系方式 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式点击本人照片即可显示WX→官方认证
一、前言
通过五个章节的分析目前对于一维卡尔曼滤波有了一定层次的理解这里先给出上篇博客推导出来的结论(卡尔曼五大公式) ① x ˇ k f x ^ k − 1 ② σ X k − f 2 σ X k − 1 σ Q k − 1 (01) \color{red} ①\tag{01}\check x_{k} f\hat x_{k-1}~~~~~~~~~~~~~~~②\sigma^{-}_{X_{k}}f^2\sigma_{X_{k-1}}^{}\sigma_{Q_{k-1}} ①xˇkfx^k−1 ②σXk−f2σXk−1σQk−1(01) ③ k k h σ X k − h 2 σ X k − σ R k (02) \color{red} \tag{02}③k_k\frac{h \sigma_{X_k}^{-} }{h^{2} \sigma_{X_k}^{-} \sigma_{R_k}} ③kkh2σXk−σRkhσXk−(02) ④ x ^ k k k ( y k − h x ˇ ) x ˇ ⑤ σ X k ( 1 − k k ) σ X k − (03) \color{red} \tag{03} ④\hat x_{k}k_k(y_k-h\check x)\check x~~~~~~~~~~~~~~~~~~⑤\sigma^_{X_{k}}(1-k_k) \sigma_{X_k}^{-} ④x^kkk(yk−hxˇ)xˇ ⑤σXk(1−kk)σXk−(03) 上面的五个式子很明显是递推的若假设已知 x ^ 0 \hat x_0 x^0、 σ X 0 \sigma_{X_{0}}^ σX0、以及各个时刻观测 y k y_k yk则可推导出出 x ^ k \hat x_k x^k、 σ X k \sigma_{X_{k}}^ σXk如下 【 x ^ 0 , σ X 0 , y 1 】 → 【 x ^ 1 , σ X 1 , y 2 】 → ⋯ → 【 x ^ k , σ X k 】 (04) \color{Green} \tag{04}【\hat x_0,\sigma_{X_{0}}^,y_1】→【\hat x_1,\sigma_{X_{1}}^,y_2】→\cdots→【\hat x_k,\sigma_{X_{k}}^】 【x^0,σX0,y1】→【x^1,σX1,y2】→⋯→【x^k,σXk】(04)该篇本博客主要是进行编程实践为了公式与源码更好的对应起来对上述公式公式进行改写因为编程中通常需要进行模块下所以代码中会实现一个函数该函数只完成一次递推故上5式符号简写为 ① x m i n u s f x p l u s ② σ m i n u s f 2 σ p l u s q (05) \color{red} ①\tag{05} x_ {minus} f x_{plus}~~~~~~~~~~~~~~~②\sigma_{minus}f^2\sigma_{plus}q ①xminusfxplus ②σminusf2σplusq(05) ③ k h σ m i n u s h 2 σ m i n u s r (06) \color{red} \tag{06}③k\frac{h \sigma_{minus} }{h^{2} \sigma_{minus} r} ③kh2σminusrhσminus(06) ④ x p l u s k ( y − h x m i n u s ) x m i n u s ⑤ σ p l u s ( 1 − k ) σ m i n u s (07) \color{red} \tag{07} ④ x_{plus}k(y-h x_{minus})x_{minus}~~~~~~~~~~~~~~~~~~⑤\sigma_{plus}(1-k) \sigma_{minus} ④xplusk(y−hxminus)xminus ⑤σplus(1−k)σminus(07)上式中的 r σ R k r\sigma_{R_k} rσRk(预测过程标准差主要影响收敛速度) q σ R k q\sigma_{R_k} qσRk(观测过程标准差理解为传感器精度可以通过实验获得)这两个值都是固定值迭代过程中通常不会改变。由于是编程(05) 式中的 x m i n u s x_ {minus} xminus 最终会被 (07) 式中的 x p l u s x_{plus} xplus 覆盖同理 σ p l u s \sigma_{plus} σplus 也会被覆盖。每次计算出来的 x p l u s x_{plus} xplus 与 σ p l u s \sigma_{plus} σplus 又会作为下一次的 x m i n u s x_{minus} xminus 与 σ m i n u s \sigma_{minus} σminus 进行输入。
二、C一维示例
是一个程序 假设有这样一条曲线 y 0.5 x 2 8 y0.5x^28 y0.5x28 现在以其为真值当然实际应用中我们是不知道的这里是为了模拟观测数据。观测数据在真值的基础上叠加一个高斯噪声 N ( 0 , 100 ) N(0,100) N(0,100)也就是公式推导中的 y k y_k yk。总之通过这种方式拿到了一批带有高斯噪声的 y k y_k yk 数据。
