C:\ti\mmwave_automotive_toolbox_2_1_2\labs\lab0002_short_range_radar\src\dss\Extended_Kalman_Filter_xyz
function computeHmat()
#if KF_2D
/* Only six elements of the hMat need to computed for KF_2D */
hMat[0] = stateVecRrd[iSIN_AZIM]; /*azim */
hMat[1] = statevec_xyz[iY] * invrange; /* y/r */
hMat[2] = (statevec_xyz[iXd] - (stateVecRrd[iSIN_AZIM] * stateVecRrd[iRANGE_RATE]))*invrange; /* xd/r-(azim*rd)/r */
hMat[3] = (statevec_xyz[iYd] - (stateVecRrd[iRANGE_RATE] * hMat[1]))*invrange; /* yd/r-(rd*y)/r^2 */
hMat[4] = (1.0f - (stateVecRrd[iSIN_AZIM] * stateVecRrd[iSIN_AZIM]))*invrange; /* 1/r-azim^2/r */
hMat[5] = -(stateVecRrd[iSIN_AZIM] * hMat[1])*invrange; /* -(azim*y)/r^2 */
I wonder hMat[4] = (1.0f - (stateVecRrd[iSIN_AZIM] * stateVecRrd[iSIN_AZIM]))*invrange; /* 1/r-azim^2/r */
should be
hMat[4] = (2.0f - (stateVecRrd[iSIN_AZIM] * stateVecRrd[iSIN_AZIM]))*invrange; /* 2/r-azim^2/r */
the reason is when H mat is a 3*4 matrix
hMat[0] hMat[1] 0 0
hMat[2] hMat[3] hMat[0] hMat[0]
hMat[4] hMat[5] 0 0
then Observation Equation should be
Range hMat[0] hMat[1] 0 0 X
Range_rate = hMat[2] hMat[3] hMat[0] hMat[0] * Y
Sin_Azim hMat[4] hMat[5] 0 0 Xd
Yd
and we know that Sin_azim = X/r and X*X+Y*Y = r*r.
Sin_Azim = hMat[4] * X + hMat[5] * Y which happen to be zero.
So I wonder if hMat[4] should be = (2.0f - (stateVecRrd[iSIN_AZIM] * stateVecRrd[iSIN_AZIM]))*invrange; /* 2/r-azim^2/r */