//Kalfilter滤波数据处理 /** * @brief PrcTools::Kalfilter * @param v 输出值 * @param x 上一个历元的解,InitX:X的初始值,不知道初始值就输入零向量,对应XKF值 * @param Qx 上一个历元接对应的方差,P * @param F 状态转移矩阵 1 * @param Qw 协方差矩阵,反映状态受外界影响的大小Q * @param B 系数阵,观测矩阵H * @param R 观测值的方差阵,反映测量的精度,取决于传感器 * @param l 输入测量得到的数据Z */ void Kalfilter( VectorXd *v, VectorXd *x, MatrixXd *Qx, const MatrixXd &F, const MatrixXd &Qw, const MatrixXd &B, const MatrixXd &R, const VectorXd &L) { (*x) = F*(*x);//一步预测 MatrixXd M = F*(*Qx)*F.transpose() + Qw;//一步预测误差方差阵P MatrixXd K = (M*B.transpose())*(B*M*B.transpose()+R).inverse();//滤波增益矩阵(权重) MatrixXd I = MatrixXd::Identity(Qx->rows(), Qx->cols());//单位矩阵 (*Qx) = (I-K*B) * M; (*Qx) = 0.5 * ((*Qx)+Qx->transpose()); (*v) = L - B*(*x); (*x) = (*x) + K*(*v); //TODOu: Deal with large KV by iteration. (*v) = B*(*x) - L; }
原文链接: https://www.cnblogs.com/yunliuh/p/13307827.html
欢迎关注
微信关注下方公众号,第一时间获取干货硬货;公众号内回复【pdf】免费获取数百本计算机经典书籍;
也有高质量的技术群,里面有嵌入式、搜广推等BAT大佬
原创文章受到原创版权保护。转载请注明出处:https://www.ccppcoding.com/archives/366350
非原创文章文中已经注明原地址,如有侵权,联系删除
关注公众号【高性能架构探索】,第一时间获取最新文章
转载文章受原作者版权保护。转载请注明原作者出处!