//============================================================================//
//== 卡尔曼滤波 ==//
//============================================================================//
//==入口参数: 无 ==//
//==出口参数: 无 ==//
//==返回值: 无 ==//
//============================================================================//
void KalMan(void)
{
unsigned char i;
unsigned short j;
srand(SEED);
for (i=0; i<X_LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp2[i]; //零值初始化
}
for (i=0; i<P_LENGTH; i++)
{
tCov.PPreOpt[i] = Temp4[i]; //零值初始化
}
for (j=0; j<N; j++)
{
Watch1[j] = sin(2*3.14159265/100.0*j);
Y[0] = Watch1[j] + Random1(0, 0.4);
Watch2[j] = Y[0];
MatrixMul(A, tOpt.XPreOpt, X, A_ROW, X_ROW, X_COLUMN); // 基于系统的上一状态而预测现在状态; X(k|k-1) = A(k,k-1)*X(k-1|k-1)
MatrixCal1(A, tCov.PPreOpt, Temp4, SYS_ORDER);
MatrixAdd(Temp4, Q, P, P_ROW, P_COLUMN); // 预测数据的协方差矩阵; P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q
MatrixCal2(C, P, Temp1, C_ROW, C_COLUMN);
MatrixAdd(Temp1, R, Temp1, R_ROW, R_COLUMN);
Gauss_Jordan(Temp1, C_ROW);
MatrixTrans(C, Temp2, C_ROW, C_COLUMN);
MatrixMul(P, Temp2, Temp22, P_ROW, C_COLUMN, C_ROW);
MatrixMul(Temp22, Temp1, K, P_ROW, C_ROW, C_ROW); // 计算卡尔曼增益; K(k) = P(k|k-1)*C' / (C(k)*P(k|k-1)*C(k)' + R)
MatrixMul(C, X, Temp1, C_ROW, X_ROW, X_COLUMN);
MatrixMinus(Y, Temp1, Temp1, Y_ROW, Y_COLUMN);
MatrixMul(K, Temp1, Temp2, K_ROW, Y_ROW, Y_COLUMN);
MatrixAdd(X, Temp2, tOpt.XNowOpt, X_ROW, X_COLUMN); // 根据估测值和测量值计算当前最优值; X(k|k) = X(k|k-1)+Kg(k)*(Y(k)-C*X(k|k-1))
MatrixMul(K, C, Temp4, K_ROW, C_ROW, C_COLUMN);
MatrixMinus(I, Temp4, Temp4, I_ROW, I_COLUMN);
MatrixMul(Temp4, P, tCov.PNowOpt, I_ROW, P_ROW, P_COLUMN); // 计算更新后估计协防差矩阵; P(k|k) =（I-Kg(k)*C）*P(k|k-1)
for (i=0; i<X_LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
}
for (i=0; i<P_LENGTH; i++)
{
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
Watch3[j] = tOpt.XNowOpt[0];
}//end of for
}
//============================================================================//
//== 卡尔曼滤波 ==//
//============================================================================//
//==入口参数: 无 ==//
//==出口参数: 无 ==//
//==返回值: 无 ==//
//============================================================================//
void KalMan(u16* in,u16* out)
{
unsigned char i;
// unsigned short k;
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp1[i]; //零值初始化
}
for (i=0; i<LENGTH; i++)
{
tCov.PPreOpt[i] = Temp2[i]; //零值初始化
}
// for (k=0; k<N; k++)
// {
Z[0] = in[0];//(float)ADCSampling();
MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系统的上一状态而预测现在状态; X(k|k-1) = F(k,k-1)*X(k-1|k-1)
MatrixCal(F, tCov.PPreOpt, Temp1, ORDER);
MatrixAdd(Temp1, Q, P, ORDER, ORDER); // 预测数据的协方差矩阵; P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q
MatrixCal(H, P, Temp1, ORDER);
MatrixAdd(Temp1, R, Temp1, ORDER, ORDER);
Gauss_Jordan(Temp1, ORDER);
MatrixTrans(H, Temp2, ORDER, ORDER);
MatrixMul(P, Temp2, Temp3, ORDER, ORDER, ORDER);
MatrixMul(Temp1, Temp3, K, ORDER, ORDER, ORDER); // 计算卡尔曼增益; Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
MatrixMul(H, X, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(Z, Temp1, Temp1, ORDER, ORDER);
MatrixMul(K, Temp1, Temp2, ORDER, ORDER, ORDER);
MatrixAdd(X, Temp2, tOpt.XNowOpt, ORDER, ORDER); // 根据估测值和测量值计算当前最优值; X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
MatrixMul(K, H, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(I, Temp1, Temp1, ORDER, ORDER);
MatrixMul(Temp1, P, tCov.PNowOpt, ORDER, ORDER, ORDER); // 计算更新后估计协防差矩阵; P(k|k) =（I-Kg(k)*H）*P(k|k-1)
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
out[0]=(u16)(tOpt.XNowOpt[0]);
// }
}
void OpMatrixMinusMatrix (TomVM& vm) {
// Matrix - Matrix
// Left matrix at reg2, right matrix at reg1
if (!ReadMatrix (vm, vm.Reg2 ().IntVal (), m1)
|| !ReadMatrix (vm, vm.Reg ().IntVal (), m2))
return;
// Add them
vmReal result [16];
MatrixMinus (m1, m2, result);
// Return as temporary matrix
vm.Reg ().IntVal () = FillTempRealArray2D (vm.Data (), vm.DataTypes (), 4, 4, result);
}
