/// <summary>
/// 在 GNSS 动态导航中的应用
/// </summary>
protected void Motion()
{
List <string> paramNames = new List <string>()
{
"X", "Y", "Z", "dX", "dY", "dZ"
};
///参数
List <Vector> satXyzs = new List <Vector>();
Vector siteXyz = new Vector(3);
Vector L = new Vector(6); //观测值
double interval = 30; //时间间隔（单位：秒）
//将瞬时加速度作为随机干扰，
//状态向量， 位置和速度 X = trans([x, y, z, vx, vy, vz])
//噪声向量， 为加速度 Noise = trans(ax, ay, az])
//纯量形式， y = y0 + t * v0 + 0.5 * a0 * a0 * t
// v = v0 + a0 * t
/// 状态方程
/// ┌I Δt┐ ┌0.5*Δt*Δt┐
/// X(k) = │ │X(k-1) + │ │Ω(k-1)
/// └0 I ┘ └ Δt ┘
/// 观测方程
/// L(k) = A(k) X(k) + Ω(k)
/// 其中，L(k)为 k 时刻系统的 C/A 码观测向量
//常用量
double[][] identity3x3 = MatrixUtil.CreateIdentity(3);
int satCount = satXyzs.Count;
//状态方程状态转移矩阵 Φ（k,k-1）
// double[][] trans = MatrixUtil.CreateIdentity(6);
ArrayMatrix trans = new ArrayMatrix(6, 6);
MatrixUtil.SetSubMatrix(trans.Array, MatrixUtil.GetMultiply(identity3x3, interval), 0, 3);
//状态方程噪声向量Ω(k-1)的系数阵
double[][] coeef_noise = MatrixUtil.Create(6, 3);
MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, 0.5 * interval * interval));
MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, interval), 3);
//观测方程系数矩阵 A,每个时刻不同
ArrayMatrix coeef_obs = new ArrayMatrix(satCount, 6);
for (int i = 0; i < satCount; i++)
{
IVector differXyz = (satXyzs[i] - siteXyz);
coeef_obs[i, 0] = differXyz.GetCos(0);//.CosX;
coeef_obs[i, 1] = differXyz.GetCos(1);
coeef_obs[i, 2] = differXyz.GetCos(2);
}
KalmanFilter lastKf = null;
//Vector lastParams = lastKf.Corrected ?? null;
//IMatrix lastParamCova = lastKf.Corrected.InverseWeight ?? null;
//KalmanFilter kf = new KalmanFilter(coeef_obs, L, trans, lastParams, lastParamCova);
//kf.Process();
}
/// <summary>
/// 构造函数
/// </summary>
public RecursiveAdjuster()
{
NormalEquationSuperposer = new NormalEquationSuperposer();
MatrixAdjuster = new KalmanFilter();
this.StepOfRecursive = StepOfRecursive.SuperposOfConstNeq;
}
