adaptiveoptimaser.h

InnovLabSimu在vc++下实现

#pragma once
/********************************************************************
	created:	2008/01/12
	created:	12:1:2008   16:46
	filename: 	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt\AdaptiveOptimaser.h
	file path:	c:\boosttest\MyTargetTrack\MyTargetTrack\UncentedKalmanFilt
	file base:	AdaptiveOptimaser
	file ext:	h
	author:		GaoYang
	
	purpose:	本类为自适应卡尔曼滤波的自适应部分,本类实现了协方差匹配思想的自适应调整滤波器中的模型噪声Q阵,事实上
				本类中的调整算法还很不完善,Q始终人为是对角阵,且调整时将滤波器中的4个状态量:
				distance,angle,dis Velocity,Angular velocity,
				对应的对角员的调整步长分为两组重复值,即Q阵中distance与dis Velocity的对应项的调整步长是一样的,而angle与
				Angular velocity对应项的调整步长是一样的,而且由于是对角阵并不考虑协方差项的影响
*********************************************************************/
#include "UKF_COMMAN_HEAD.h"
#include "OptimizerMethod.h"
typedef boost::numeric::ublas::vector<double>RESIDUAL_STYLE;
#define STABLE_THRES 0.01//寻优所得调整步长变化值（即最终调整步长不包含+1部分）小于此值时认为寻优过程已进入稳定状态
class CAdaptiveOptimaser:virtual public COptimizerMethod,virtual public BOOST_DEBUG_TOOL
{
public:
	CAdaptiveOptimaser(void);
	CAdaptiveOptimaser(int CovEstNum,size_t ObserveSize,size_t StateSize);
public:
	
	~CAdaptiveOptimaser(void);
	/**@brief初始化寻优器，把模型噪声初始方差，以及判断是否退出稳态的参数传进来
	*
	*@parm NodelNoiseOriginalVal 模型噪声初始方差
	*@parm StableDifAmplitudeParm 进入稳态后，将当前方差差乘此系数当作判断是否退出稳态的标准
	*@output 返回初始化是否成功,本来需要判断NodelNoiseOriginalVal是否正定的,但这里没做
	*@todo 返回初始化是否成功,本来需要判断NodelNoiseOriginalVal是否正定的,但这里没做
	*/
	bool initAdapter(VARIANCE& NodelNoiseOriginalVal,float StableDifAmplitudeParm);
public:
	VARIANCE m_mNodelNoiseOriginalVal;///<模型噪声初始方差,initAdapter中赋值。
	float m_fStableDifAmplitudeParm;///<进入稳态后，将当前方差差乘此系数当作判断是否退出稳态的标准


	VARIANCE m_mNewModelNoise;///<ModelNoiseAdapter的寻优结果，需要在构造函数中指定维数需要在init函数中初始化
	int m_iCovEstNum;///<计算理论方差阵时，以平均计算前几次方差阵来代替，此即为平均计算所包含的方差阵个数
	VARIANCE m_mTherioticalResCov;///<计算出来的残差协方差理论值
	VARIANCE m_calResidualCoviarance;///<残差向量计算得到的方差
	VARIANCE m_SumOfAllResCov;///<m_mResidualCovStack内的所有残差向量之和
	std::vector<VARIANCE> m_mCovDifStack;///<残差协方差理论值与实际值间的差值的存储区域，固定存3个差值
	size_t m_iNumOfCovDif;///<已计算出的方差差值数目
	std::vector<VARIANCE> m_mResidualCovStack;///<平均计算时用的残差协方差存储区，将m_iCovEstNum个残差协方差存于其中并实时更新，需在初始化函数中指定维数
	int m_iNumOfInputResidual;///<输入的残差向量个数
#ifndef USE_TRADITIONAL_AKF
public:
	/**@brief基于协方差匹配法的模型噪声阵寻优器,给出寻优调整后的模型噪声阵的,使输入的残差协方差阵实际值与理论值间误差最小
	*输入量均为卡尔曼滤波更新过程中计算的中间量
	*
	*@parm Residual 输入的残差向量（预测观测值与实际观测值之差）
	*@parm ResidualCoviarance 输入的残差向量的协方差阵，也是预测协方差镇
	*@output 输出为新的模型噪声阵供下一次使用.输出仅为引用，实际量仍然存储在本类成员变量m_mNewModelNoise中
	*/
	VARIANCE& ModelNoiseAdapter(RESIDUAL_STYLE Residual, VARIANCE ResidualCoviarance);
	VARIANCE m_mPreDeltaStep;///<前一次的步长变化值幅度
	VARIANCE m_mCurModeNoiseModStep;///<当前模型噪声方差阵的调整值
	VARIANCE m_mAlterStepVal;///<寻优后给出的结果，的步长变化值
protected:
	//////////////////////////////////////////////////////////////////////////
	
	bool CalculateTheroticalCovOfResidual(RESIDUAL_STYLE& Residual,VARIANCE ResidualCov);
		
	VARIANCE m_mDif_ResCov2TherResCov;///<残差协方差理论值与实际值间的差值
public:


		
	
	/**@brief 计算误差函数的当前时刻，前一时刻两个导数
	*
	*事实上计算用是残差协方差当前时刻值-前一时刻值，来代表导数，本应该在差值基础上除时间长度的，但考虑RPPOD算法仅用到了导数的符号，所以简化了
	*/
	bool CalculateDifOfError();
	VARIANCE m_mCurrentErDif;///<误差函数当前时刻的导数，或斜率
	VARIANCE m_mPreErDif;///<误差函数前一时刻的导数，或斜率
	
	/**@brief 检测是否寻优算法调整步长已经入稳态，是否已推出稳态，进入则返回true
	*
	*算法本身通过检测本次寻优后的调整步长来确认是否进入稳态，比如经过寻优，步长的调整幅度已经非常小了那么表示寻优进入稳态
	*同时记录稳态时残差协方差的对角元，作为是否推出稳态的判定依据.
	*output 返回m_bStableTag
	*/
	bool OptmiseInStableCheck(VARIANCE& CovAlterStep);
	//bool m_bStableTag;///<寻优是否进入稳态的标志位
	bool m_bStableTag[2];///<对角线上的每个元素对应一个寻优是否进入稳态的标志位
	/*注意,此处本不该声明成2维大小,应随构造函数中指定的维数而定.*/
	double m_dStableDifThres[2];///<是否跳出稳态的阈值, 
#endif


public:
#define MAX_MODEL_NOISE 200
#define MIN_MODEL_NOISE 1e-5
	// 限制模型噪声不太大也别太小,若超限，则限制住值并返回真，否则返回假
	bool LimitModNoiseInSafeVal(double&ModelNoise)
	{
		if (ModelNoise<MIN_MODEL_NOISE)
		{
			ModelNoise=MIN_MODEL_NOISE;
			return true;
		}
		else if (ModelNoise>MAX_MODEL_NOISE)
		{
			ModelNoise=MAX_MODEL_NOISE;
			return true;
		}	
		return false;
	}


public:
	// 对寻优调整的结果（模型噪声）进行限幅，不能太大也不能太小，输入为寻优的结果矩阵的引用
	void LimitModifiedModelNoise(VARIANCE& ModifyResult)
	{
		for (unsigned i=0;i< ModifyResult.size1();i++)
		{
			unsigned j=i;
			LimitModNoiseInSafeVal(ModifyResult(i,j));
		}
	}
#ifdef USE_TRADITIONAL_AKF
	/**
	*@brief 重载函数，依靠输入的残差仅计算理论残差协方差
	*/
	bool CalculateTheroticalCovOfResidual(RESIDUAL_STYLE& Residual);
	/**
	*@brief 重载函数，依靠传统统自适因调节器计算模型噪声接口函数
	*@parm ResidualUKF　更新残差
	*@parm PredictVar　预测后状态的方差阵
	*@parm StateVar　更新后最终状态的方差阵
	*@parm InnovationVar　残差的协方差阵实际值
	*@parm KalmanGain　卡尔曼滤波的增益阵
	*/
	VARIANCE& CAdaptiveOptimaser::ModelNoiseAdapter(RESIDUAL_STYLE Residual,VARIANCE& PredictVar,VARIANCE& StateVar,matrix_type& KalmanGain);
	/**
	*@brief 运用传统自适因调节器计算模型噪声，通过公式推导及极大似然估计得到的公式
	*@parm PredictVar　预测后状态的方差阵
	*@parm StateVar　更新后最终状态的方差阵
	*@parm InnovationVar　残差的协方差阵实际值
	*@parm KalmanGain　卡尔曼滤波的增益阵
	*/
	void ConventionalAdapter(VARIANCE& PredictVar,VARIANCE& StateVar,VARIANCE& InnovationVar,matrix_type& KalmanGain)
	{
		m_mNewModelNoise=prod(matrix_type(prod(KalmanGain,InnovationVar)),trans(KalmanGain));//+StateVar-PredictVar;
			//Q=Cdx+StateVar-PredictVAR;
	}
#endif
};