基于UKF的两轮自平衡机器人姿态最优估计研究

针对扩展卡尔曼滤波器(EKF)设计困难并且容易发散的问题，提出基于采样卡尔曼滤波(UKF)的方法解决滤波器设计及收敛问题，并补偿低成本的惯性传感器陀螺仪和加速度计的误差，从而得到机器人姿态的最优估计．将滤波后的模型应用到两轮自平衡机器人系统，实验结果表明UKF参数设计简单，姿态估计误差小于EKF，方差估计优于EKF，估计精度、计算量基本与EKF相当．因此，UKF能够满足两轮自平衡机器人快速机动过程中的实时姿态估计要求．     

A UD factorization‐based nonlinear adaptive set‐membership filter for ellipsoidal estimation

The extended set‐membership filter (ESMF) for nonlinear ellipsoidal estimation suffers from numerical instability, computation complexity as well as the difficulty in filter parameter selection. In this paper, a UD factorization‐based adaptive set‐membership filter is developed and applied to nonlinear joint estimation of both time‐varying states and parameters. As a result of using the proposed UD factorization, combined with a new sequential and selective measurement update strategy, the numerical stability and real‐time applicability of conventional ESMF are substantially improved. Furthermore, an adaptive selection scheme of the filter parameters is derived to reduce the computation complexity and achieve sub‐optimal estimation. Simulation results have shown the efficiency and robustness of the proposed method. Copyright © 2007 John Wiley & Sons, Ltd.     

基于UD-EKF自主光学导航方法仿真

针对探测器自主光学导航系统滤波算法鲁棒性的要求,通过对深空探测器巡航段自主轨道确定方法的研究,提出利用星敏感器及光学导航相机,通过对小行星附近多颗小天体夹角的测量,并结合几何关系,来实时确定飞行器轨道状态的一种基于UD分解的扩展卡尔曼滤波的光学自主导航的方案,并给出其UD分解构造算法;通过对协方差矩阵的UD分解来避免对协方差矩阵的直接求逆而导致的计算机舍入误差过大从而滤波发散的情况,通过matlab仿真验证了该方案可行性,节省了星载计算机的内存限制了舍入误差的增长,达到了较好的滤波效果.     

四元数卡尔曼滤波组合导航算法性能分析

为分析四元数卡尔曼滤波组合导航算法在飞行器姿态估计中的性能,在建立四元数卡尔曼滤波观测方程、状态方程和方差计算模型的基础上,分别设计了陀螺/加速度计/磁强计组合导航仿真算例和陀螺/加速度计初始对准实验,比较了四元数卡尔曼滤波组合导航算法相较于传统扩展卡尔曼滤波组合导航算法在计算量、收敛性、收敛速度、收敛精度方面的性能.分析结果表明该滤波器无须扩展卡尔曼滤波器的线性化过程,计算量小,算法实现简单;收敛性和收敛速度均优于扩展卡尔曼滤波器.收敛精度较扩展卡尔曼滤波器高出约两个数量级,但收敛过程中存在一个比扩展卡尔曼滤波器精度低的时间区间.     

Hyperbolic‐singular‐value‐decomposition‐based square‐root accurate continuous‐discrete extended‐unscented Kalman filters for estimating continuous‐time stochastic models with discrete measurements

This paper presents novel square‐root accurate continuous‐discrete extended‐unscented Kalman filtering (ACD‐EUKF) algorithms for treating continuous‐time stochastic systems with discrete measurements. The time updates in such methods are fulfilled as those in the extended Kalman filter whereas their measurement updates are copied from the unscented Kalman filter. All this allows accurate predictions of the state mean and covariance to be combined with accurate measurement updates. The main weakness of this technique is the need for the Cholesky decomposition of predicted covariances derived in time‐update steps. Such a factorization is highly sensitive to numerical integration and round‐off errors committed, which may result in losing the covariance's positivity and, hence, failing the Cholesky decomposition. The latter problem is usually solved in the form of square‐root filtering implementations, which propagate not the covariance matrix but its square root instead. Here, we devise square‐root ACD‐EUKF methods grounded in the singular value decomposition (SVD). The SVD rooted in orthogonal transforms is applicable to any ACD‐EUKF with nonnegative weights, whereas the remaining ones, which can enjoy negative weights as well, are treated by means of the hyperbolic SVD based on J‐orthogonal transforms. The filters constructed are presented in a concise algorithmic form, which is convenient for practical use. Their two particular versions grounded in the classical and cubature unscented Kalman filtering parameterizations are examined in severe conditions of tackling a radar tracking problem, where an aircraft executes a coordinated turn. These are also compared to their non‐square‐root predecessor and other methods within the target tracking scenario with ill‐conditioned measurements.     

The Cramér-Rao estimation error lower bound computation for deterministic nonlinear systems

For continuous-time nonlinear deterministic system models with discrete nonlinear measurements in additive Ganssian white noise, the extended Kalman filter (EKF) convariance propagation equations linearized about the true unknown trajectory provide the Cramér-Rao lower bound to the estimation error covariance matrix. A useful application is establishing the optimum filter performance for a given nonlinear estimation problem by developing a simulation of the nonlinear system and an EKF linearized about the true trajectory.     

Adaptive Iterated Extended KALMAN Filter for Relative Spacecraft Attitude and Position Estimation

This paper presents a novel adaptive iterated extended Kalman filter (AIEKF) for relative position and attitude estimation, taking into account the influence of model uncertainty. Considering a nonlinear stochastic discrete‐time system with unknown disturbance, the AIEKF algorithm adopts the Gauss‐Newton iterative optimization steps to implement a maximum a posteriori (MAP) estimation, and the switch‐mode combination technique is used to achieve the adaptive capability. The mean‐square estimation error (MSE) of the state estimate is derived. It is proved that the AIEKF can yield a smaller MSE than that of the traditional extended Kalman filter (EKF) or iterated extended Kalman filter (IEKF). The performance advantage of the AIEKF is illustrated via Monte Carlo simulations on a typical relative position and attitude estimation application. Through comparisons in different scenarios, the presented algorithm is shown to improve adaptability and ensure estimation accuracy.     

Ranging-aided relative navigation of multi-platforms

This paper proposes a relative attitude and distance estimation algorithm based on pairwise range measurements between vehicles as well as inertial measurement of each platform. The solution of Wahba''s Problem is introduced to compute the relative attitude between multi-platforms with the sampled pairwise ranges, in which the relative distance estimation is derived and the estimation error distributions are analyzed. An extended Kalman filter is designed to fuse the estimated attitude and distance with the inertial measurement of each platform. The relative poses between platforms are determined without any external aided measurement. To show this novelty, a real testbed is constructed by our research lab. And the experiment results are positive.     

Computation of the factorized error covariance of the differencebetween correlated estimators

A state estimation problem where some of the measurements may be common to two or more data sets is considered. Two approaches for computing the error covariance of the differences between filtered estimates (for each data set) are discussed. The first algorithm is based on postprocessing of the Kalman gain profiles of two correlated estimators. It uses UD factors of the covariance of the relative error. The second algorithm uses a square root information filter applied to relative error analysis. In the absence of process noise, the square root information filter is computationally more efficient and more flexible than the Kalman gain (covariance update) method. Both the algorithms (covariance and information matrix based) are applied to a Venus orbiter simulation and their performances are compared     

基于SVD 的改进抗差UKF 算法及在组合导航中的应用

针对GPS/INS组合导航中因观测异常导致系统状态先验信息矩阵失去对称正定性,及传统等价权函数抗差算法易遇到病态矩阵,引起滤波性能下降的问题,提出一种基于奇异值分解的改进抗差UKF算法。该算法克服了先验协方差矩阵负定性变化,通过判断矩阵病态性实现智能选取抗差策略。最后利用车载实测数据进行验证,所得结果表明, SVD-UKF导航解精度稍优于EKF算法,改进的抗差策略能够极大减弱单独、连续以及混合的观测异常对导航解的影响,提高了导航解精度和可靠性。     

Robust unscented Kalman filter with adaptation of process and measurement noise covariances

Unscented Kalman filter (UKF) has been extensively used for state estimation of nonlinear stochastic systems, which suffers from performance degradation and even divergence when the noise distribution used in the UKF and the truth in a real system are mismatched. For state estimation of nonlinear stochastic systems with non-Gaussian measurement noise, the Masreliez–Martin extended Kalman filter (EKF) gives better state estimates in relation to the standard EKF. However, the process noise and the measurement noise covariance matrices should be known, which is impractical in applications. This paper presents a robust Masreliez–Martin UKF which can provide reliable state estimates in the presence of both unknown process noise and measurement noise covariance matrices. Two numerical examples involving relative navigation of spacecrafts demonstrate that the proposed filter can provide improved state estimation performance over existing robust filtering approaches. Vision-aided robot arm tracking experiments are also provided to show the effectiveness of the proposed approach.     

基于MAEKF算法的航姿参考系统设计

构建了以低成本MEMS陀螺仪、加速度计和磁传感器组合的航姿参考系统,提出了一个乘性自适应扩展卡尔曼滤波算法.取乘性误差四元数和陀螺仪误差作为状态量,基于重力场和磁场构造了量测矢量,用于修正航姿数据.并采用准确量测法,给滤波器加入了四元数的归一化约束,最后给出了基于新息的估计量测噪声方差矩阵的公式.通过仿真和试飞验证,表明本文设计的低成本的航姿参考系统能够提供比较准确的航姿信息.与常规的扩展卡尔曼滤波器比较,本文设计的乘性自适应扩展卡尔曼滤波算法有效提高了系统的精度和稳定性,并且具有较好的鲁棒性.     

用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态

在较大初始姿态误差角下, 针对捷联惯导/CCD星敏感器(strap-intertial navigation system/CCD star sensor, SINS/CCD)姿态估计系统扩展卡尔曼滤波(extended Kalman filter, EKF)算法精度下降的问题, 提出了基于四元数的状态切换无迹卡尔曼滤波算法. 通过状态实时切换降低了全维无迹卡尔曼滤波(unscented Kalman filter, UKF)的维数, 减小了计算复杂度, 提高了系统的实时性. 文中采用基于特征向量求解的代价函数法计算四元数均值避免了UKF算法中四元数规范化的限制; 利用乘性误差四元数表示姿态更新点与估计点之间的距离, 解决了四元数协方差阵奇异性问题. 仿真实验结果表明: 与EKF相比, 该算法在精度上有较大提高; 与全维UKF算法和修正罗德里格斯参数UKF算法相比, 该算法精度相当但估计时间均有不同程度的减少.     

Square‐Root Cubature‐Quadrature Kalman Filter

In this paper, an on‐going work introducing square‐root extension of cubature‐quadrature based Kalman filter is reported. The proposed method is named square‐root cubature‐quadrature Kalman filter (SR‐CQKF). Unlike ordinary cubature‐quadrature Kalman filter (CQKF), the proposed method propagates and updates square‐root of the error covariance without performing Cholesky decomposition at each step. Moreover SR‐CQKF ensures positive semi‐definiteness of the state covariance matrix. With the help of two examples we show the superior performance of SR‐CQKF compared to EKF and square root cubature Kalman filter.     

基于UD分解的自适应扩展集员估计方法

用于非线性椭球估计的扩展集员算法在实际应用中存在着数值稳定性差、计算复杂度高以及滤波器参数难以选择等问题. 本文提出了一种基于 UD 分解的自适应扩展集员估计算法, 用于解决非线性系统时变状态和参数的联合估计和定界问题. 新算法将 UD 分解与序列更新和选择更新策略结合起来, 改进了传统扩展集员算法的数值稳定性和实时性能; 同时, 对滤波器参数进行自适应选择以进一步降低计算复杂度并达到次优估计结果. 仿真实验表明了该算法的有效性和鲁棒性.     

自适应渐消有偏扩展卡尔曼滤波在目标跟踪中的应用

针对机动目标跟踪过程观测矩阵病态导致扩展卡尔曼滤波算法跟踪效果不佳的问题,提出一种自适应渐消有偏扩展卡尔曼滤波算法。该算法以扩展卡尔曼滤波为基本框架,并借鉴Gauss-Markov模型的思想以解决观测矩阵病态问题。算法根据状态估计均方误差最小条件求得有偏因子,以降低病态观测矩阵对滤波估计的影响;根据滤波发散判据提出一种新的渐消因子估计方法,以实时调整预测协方差矩阵,从而改善滤波增益并有效提高目标跟踪精度。仿真结果表明,改进算法比传统扩展卡尔曼滤波对目标跟踪的精度有较大提高,同时稳定性更好。     

基于SVD的推广卡尔曼滤波及其在飞行状态和参数估计中的应用

基于矩阵的奇异值分解技术，本文提出一种鲁棒推广卡尔曼波新算法，并将该算法应用于飞行状态和参数估计中，该算法不仅具有很好的数值稳定性，而且无需任何变换即可处理相关噪声，且适于并行计算。两种不同型号飞机飞行数据计算结果表明；与ＥＫＦ相比，本文算法对不同初始值的不同噪声均可获得更准确的估计结果，并且对飞机机动形式、噪声水平，数据长度等要求不高，收敛性好。     

基于模糊神经网络优化扩展卡尔曼滤波的锂离子电池荷电状态估计

电池荷电状态(state of charge,SOC)的精确估计是判断电池是否过充或过放的重要依据,是电动汽车安全、可靠运行的重要保障.传统基于扩展卡尔曼滤波(extended Kalman filter,EKF)的SOC估计方法过度依赖于精确的电池模型,并且要求系统噪声必须服从高斯白噪声分布.为解决上述问题,基于模糊神经网络(fuzzy neural network,FNN)建立模型误差预测模型,并藉此修正扩展卡尔曼滤波测量噪声协方差,以实现当模型误差较小时对状态估计进行测量更新,而当模型误差较大时只进行过程更新.仿真和实验结果表明,该算法能有效消除由于模型误差和测量噪声统计特性不确定而引入的SOC估计误差,误差在1.2%以内,并且具有较好的收敛性和鲁棒性,适用于电动汽车的各种复杂工况,应用价值较高.     

EKF与互补融合滤波在姿态解算中的研究

传统的信号滤波方法不能有效的融合多传感器测量数据,或者融合中失去过去信号状态信息.针对这一问题,提出了扩展卡尔曼滤波(Extended kalman filtering,EKF)与互补滤波融合的信号处理策略.借助STM32微处理器采集MPU9250惯性测量传感器的原始数据,运用多传感器信息融合的处理算法,比较了互补滤波姿态解算结果和对互补滤波过程中所得的四元数运用EKF矫正后进行姿态解算的结果,以及互补滤波解算的欧拉角运用EKF矫正后的姿态数据.通过实验中3种解算结果与理论值的对比,得出结论:采用互补滤波会存在一定超调量,且结果波动较大,存在较大的噪声,对互补滤波过程中的四元数进行EKF滤波虽能降低解算结果的噪声,但仍存在超调量.而应用EKF矫正互补滤波解算出的欧拉角能同时解决超调量和降低噪声误差,抑制了随机波动,起到了更好的解算效果.     

Identification of process and measurement noise covariance for state and parameter estimation using extended Kalman filter

The performance of Bayesian state estimators, such as the extended Kalman filter (EKF), is dependent on the accurate characterisation of the uncertainties in the state dynamics and in the measurements. The parameters of the noise densities associated with these uncertainties are, however, often treated as ‘tuning parameters’ and adjusted in an ad hoc manner while carrying out state and parameter estimation. In this work, two approaches are developed for constructing the maximum likelihood estimates (MLE) of the state and measurement noise covariance matrices from operating input-output data when the states and/or parameters are estimated using the EKF. The unmeasured disturbances affecting the process are either modelled as unstructured noise affecting all the states or as structured noise entering the process predominantly through known, but unmeasured inputs. The first approach is based on direct optimisation of the ML objective function constructed by using the innovation sequence generated from the EKF. The second approach - the extended EM algorithm - is a derivative-free method, that uses the joint likelihood function of the complete data, i.e. states and measurements, to compute the next iterate of the decision variables for the optimisation problem. The efficacy of the proposed approaches is demonstrated on a benchmark continuous fermenter system. The simulation results reveal that both the proposed approaches generate fairly accurate estimates of the noise covariances. Experimental studies on a benchmark laboratory scale heater-mixer setup demonstrate a marked improvement in the predictions of the EKF that uses the covariance estimates obtained from the proposed approaches.