const SubMatrix MatrixPartition::operator()(int i, int j)const{
int rlo = row_start_[i];
int rhi = (i < row_max_) ? row_start_[i+1] - 1 : m_->nrow()-1;
int clo = col_start_[j];
int chi = (j < col_max_) ? col_start_[j+1] - 1 : m_->ncol()-1;
return SubMatrix(*m_, rlo, rhi, clo, chi);
}
// Assumes that there is either no exogenous variables or there is a single
// exogenous variable that is constant and equal to one. The unconditional
// mean is obtained from the reduced form companion matrix.
TDenseMatrix UnconditionalVariance(const TDenseMatrix &A0, const TDenseMatrix &Aplus, bool IsConstant)
{
int n_lags=NumberLags(A0,Aplus,IsConstant), n_vars=A0.cols;
TDenseMatrix B=ReducedForm(A0,Aplus);
if (n_lags == 0) return ConditionalVariance(A0);
TDenseMatrix C=CompanionMatrix(B,n_lags), V=BlockDiagonalMatrix(A0,n_lags),
X=V*(Identity(n_vars*n_lags) - C);
try
{
return SubMatrix(Inverse(TransposeMultiply(X,X)),0,n_vars-1,0,n_vars-1);
}
catch (dw_exception &e)
{
throw dw_exception("UnconditionalMean(): Unconditional mean does not exist");
}
}
Matrix Matrix::Adjoint() const
{
Matrix result(m_rows, m_cols);
double cofactor;
for (size_t i = 0; i < m_rows; ++i)
{
for (size_t j = 0; j < m_cols; ++j)
{
cofactor = MathFunctions::Pow(-1, static_cast<double>(i + j)) * SubMatrix(i, j).Determinant();
result.m_pDataM[i][j] = cofactor;
}
}
return result;
}
int main(void)
{
TSMatrix *a1, *a2, *result1, *result2;
a1 = InitTriple(3, 3, 3);
a2 = InitTriple(3, 3, 3);
InputValue(a1);
InputValue(a2);
ShowMatrix(a1);
ShowMatrix(a2);
if ((result1 = AddMatrix(a1, a2)) != NULL)
ShowMatrix(result1);
if ((result2 = SubMatrix(a1, a2)) != NULL)
ShowMatrix(result2);
return 0;
}
wxMatrix2D wxMatrix2D::SubCols(int start_col, int end_col) const
{
wxCHECK_MSG(Ok(), wxMatrix2D(), wxT("Invalid wxMatrix2D"));
return SubMatrix(wxRect(start_col, 0, end_col-start_col+1, M_MATRIXDATA->m_height));
}
wxMatrix2D wxMatrix2D::SubRows(int start_row, int end_row) const
{
wxCHECK_MSG(Ok(), wxMatrix2D(), wxT("Invalid wxMatrix2D"));
return SubMatrix(wxRect(0, start_row, M_MATRIXDATA->m_width, end_row-start_row+1));
}
/****************************************************************
*函 数 名: 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;
}
