Matrix & Matrix::operator /= (const Matrix &p)
{
Matrix tmp(p);
tmp.inverse();
*this = MulMatrix(*this, tmp);
return *this;
}
/****************************************************************
*函 数 名: DoEKF()
*参 数:
* pModel model: 需要进行EKF滤波的模型
* pModelParam mParam: 状态量、观测量及估计值保存地址
* pEKFParam ekfParam: KF滤波所需要的参数
*
*功 能：对model所示的模型进行EKF滤波
* 考虑了Pkk-1和Pk为对称矩阵，当求解这两项时的算法和
* 普通矩阵的乘法不一样。
*
*返 回 值:
* 成功: OK
* 输入输出指针为空：ERR_MEM_NONE
*
*作 者：申文斌
*完成日期：2011-3-22
****************************************************************/
int DoEKF(pModel model, pModelParam mParam, pEKFParam ekfParam)
{
const double T = 0.033; // 采样周期
int i, j, k; // 循环变量
// 以下为滤波过程中要用的临时变量
// 这里统一用一维数组表示矩阵,为了复用地址，这里按最大需求开辟
// 由于在没有操作系统的ARM7上不可以动态开辟空间，所以这里只能用
// 这种比较笨的方法了。
double fx[MAX_STATE]; // 保存状态方程计算值
double Xhatkk_1[MAX_STATE];
double Hk[MAX_OBS * MAX_STATE]; // 保存观测矩阵Hk
double Phikk_1[MAX_STATE * MAX_STATE];
double tmp[MAX_STATE * MAX_STATE];
double Pkk_1[MAX_STATE * MAX_STATE];
double Kk[MAX_STATE * MAX_OBS];
double PxHk[MAX_STATE * MAX_OBS];
double TranHk[MAX_STATE * MAX_OBS];
double KkTmp[MAX_OBS * MAX_OBS];
double h[MAX_OBS];
double KkxHk[MAX_STATE * MAX_STATE];
//double TranPhikk_1[MAX_STATE * MAX_STATE];
#ifdef __DEBUG__
if ( (model == NULL) || (mParam == NULL) || (ekfParam == NULL) )
{
return ERR_MEM_NONE; // 输入输出指针为空
}
#endif
if (*ekfParam->isFirst == 1) // 第一次执行滤波
{
*ekfParam->isFirst = 0;
// 将传入的状态直接当作估计值
for (i=0; i<mParam->lenState; i++)
{
ekfParam->preEst[i] = mParam->state[i];
mParam->est[i] = mParam->state[i];
}
return OK;
}//if
// EKF滤波过程
// 进一步滤波Xhatkk_1 = Xhatk_1 + T * fxu(Xhatk_1) 式(3-60)
model->fxu(ekfParam->preEst, mParam->othParam, fx);
for (i=0; i<mParam->lenState; i++)
{
fx[i] *= T;
}
AddMatrix(ekfParam->preEst, fx, Xhatkk_1, mParam->lenState, 1);
if ( model->fxu == QGFxu) // 如果状态方程为四元数的则需要归一Q
{
Norm(Xhatkk_1); // 归一Q
}
// 求得观测矩阵对状态的求导，Hk被用的大小为mParam->lenObs * mParam->lenState
model->hxDot(Xhatkk_1, Hk);
// 状态转移矩阵,离散化导致矩状态转移阵前面需要乘以T
// Phikk_1被用的大小为mParam->lenState * mParam->lenState
model->fxDot(ekfParam->preEst, mParam->othParam, Phikk_1);
for (i=0; i<mParam->lenState; i++)
{
for (j=0; j<mParam->lenState; j++)
{
Phikk_1[i * mParam->lenState + j] *= T;
}
}
// 预测误差方差矩阵Pkk_1
// 计算Pkk_1，这里考虑Pkk_1为对称矩阵
// 中间变量tmp用到的大小为mParam->lenState * mParam->lenState
MulMatrix(Phikk_1, ekfParam->Pk, tmp,
mParam->lenState, mParam->lenState, mParam->lenState);
// 这里两矩阵相乘为对称矩阵，所以可以简化
// 初始化Pkk_1,被用到的大小为mParam->lenState * mParam->lenState
for (i=0; i<mParam->lenState; i++)
{
for (j=0; j<=i; j++)
{
Pkk_1[i * mParam->lenState + j] = 0.0; // 将输出矩阵下三角初始化为0
}
}
for (i=0; i<mParam->lenState; i++) // 矩阵相乘
{
for (j=0; j<=i; j++)
{
for (k=0; k<mParam->lenState; k++)
{
// 这里乘以Phikk_1的转置
Pkk_1[i * mParam->lenState + j] +=
tmp[i * mParam->lenState + k] * Phikk_1[j * mParam->lenState + k];
}
Pkk_1[j * mParam->lenState + i] = Pkk_1[i * mParam->lenState + j]; // 对称的元素
}
}
for (i=0; i<mParam->lenState; i++)
{
// 加上Q阵，这里只要加上对角线上的元素即可
Pkk_1[i * mParam->lenState + i] += ekfParam->Q[i];
}
// 求取滤波增益矩阵 式(3-62)
TranMatrix(Hk, TranHk, mParam->lenObs, mParam->lenState);
// PxHk被用到的大小为 mParam->lenState * mParam->lenObs
MulMatrix(Pkk_1, TranHk, PxHk, mParam->lenState, mParam->lenState, mParam->lenObs);
// KkTmp被用到的大小为mParam->lenObs * mParam->lenObs
MulMatrix(Hk, PxHk, KkTmp, mParam->lenObs, mParam->lenState, mParam->lenObs);
// Hk*Pkk_1*Hk' + R，考虑到R为对角矩阵，所以不用按一般相乘计算
// 只需要加对角线元素即可
for (i=0; i<mParam->lenObs; i++)
{
KkTmp[i * mParam->lenObs + i] += ekfParam->R[i];
}
// Hk*Pkk_1*Hk' + R 对称矩阵
if ( InvSymMtrx(KkTmp, KkTmp, mParam->lenObs) != OK )
{
// 如果Hk*Pkk_1*Hk' + R奇异无法求逆
// 则将传入的状态直接当作估计值
for (i=0; i<mParam->lenState; i++)
{
ekfParam->preEst[i] = mParam->state[i];
mParam->est[i] = mParam->state[i];
}
return OK;
}
// Kk被用到的大小为mParam->lenState * mParam->lenObs
MulMatrix(PxHk, KkTmp, Kk, mParam->lenState, mParam->lenObs, mParam->lenObs);
// 状态估计
model->hx(Xhatkk_1, h);
SubMatrix( mParam->obs, h, h, mParam->lenObs, 1); // h = obs - hx(Xhatkk_1)
//----------------------- syc 2011.11.14------------------------------//
if ( model->fxu == QGFxu) // 如果状态方程为四元数的则需要调整角度
{
for(int i=0;i<3;i++)
{
if(h[i]>PI)
h[i]-=2.0*PI;
else
if(h[i]<-PI)
h[i]+=2.0*PI;
}
}
//------------------------------------------------------------------//
MulMatrix(Kk, h, ekfParam->preEst, mParam->lenState, mParam->lenObs, 1);
// ekfParam->preEst是本次求得的滤波值
AddMatrix(ekfParam->preEst, Xhatkk_1, ekfParam->preEst, mParam->lenState, 1);
if ( model->fxu == QGFxu) // 如果状态方程为四元数的则需要归一Q
{
Norm(ekfParam->preEst); // 归一Q
}
// 滤波方差矩阵
// Kk被用到的大小为mParam->lenState * mParam->lenState
MulMatrix(Kk, Hk, KkxHk, mParam->lenState, mParam->lenObs, mParam->lenState);
for (i=0; i<mParam->lenState; i++)
{
for (j=0; j<mParam->lenState; j++)
{
KkxHk[i * mParam->lenState + j] = (i == j) - KkxHk[i * mParam->lenState + j];
}
}
// MulMatrix(KkxHk, Pkk_1, ekfParam->Pk, mParam->lenState, mParam->lenState, mParam->lenState);
for (i=0; i<mParam->lenState; i++)
{
for (j=0; j<=i; j++)
{
ekfParam->Pk[i*mParam->lenState + j] = 0.0; // Pk下三角矩阵为零
}
}
for (i=0; i<mParam->lenState; i++) // 矩阵相乘
{
for (j=0; j<=i; j++)
{
for (k=0; k<mParam->lenState; k++)
{
ekfParam->Pk[i*mParam->lenState + j] +=
KkxHk[i * mParam->lenState + k] * Pkk_1[k * mParam->lenState + j];
}
// 对称的元素
ekfParam->Pk[j*mParam->lenState + i] = ekfParam->Pk[i*mParam->lenState + j];
}
}
// 将结果复制到目的地址
for (i=0; i<mParam->lenState; i++)
{
mParam->est[i] = ekfParam->preEst[i];
}
return OK;
}
Matrix & Matrix::operator *= (const Matrix &p)
{
*this = MulMatrix(*this, p);
return *this;
}