def ddres(nav, x, y, e, sat, el): """ DD phase/code residual """ _c = gn.rCST.CLIGHT nf…
- User
def ddres(nav, x, y, e, sat, el): """ DD phase/code residual """ _c = gn.rCST.CLIGHT nf = nav.nf ns = len(el) mode = 1 if len(y) == ns else 0 # 0:DD,1:SD nb = np.zeros(2len(nav.gnss_t)nf, dtype=int) Ri = np.zeros(nsnf2) Rj = np.zeros(nsnf2)
nv = 0 b = 0 H = np.zeros((ns*nf*2, nav.nx)) v = np.zeros(ns*nf*2) idx_f = [0, 1] for sys in nav.gnss_t: for f in range(nf): idx_f[f] = nav.obs_idx[f][sys] for f in range(0, nf*2): if f < nf: freq = nav.freq[idx_f[f]] # reference satellite idx = sysidx(sat, sys) if len(idx) > 0: i = idx[np.argmax(el[idx])] for j in idx: if i == j: continue if y[i, f] == 0.0 or y[j, f] == 0.0: continue # DD residual if mode == 0: if y[i+ns, f] == 0.0 or y[j+ns, f] == 0.0: continue v[nv] = (y[i, f]-y[i+ns, f])-(y[j, f]-y[j+ns, f]) else: v[nv] = y[i, f]-y[j, f] H[nv, 0:3] = -e[i, :]+e[j, :] if f < nf: # carrier idx_i = IB(sat[i], f, nav.na) idx_j = IB(sat[j], f, nav.na) lami = _c/freq v[nv] -= lami*(x[idx_i]-x[idx_j]) H[nv, idx_i] = lami H[nv, idx_j] = -lami Ri[nv] = varerr(nav, el[i], f) Rj[nv] = varerr(nav, el[j], f) nav.vsat[sat[i]-1, f] = 1 nav.vsat[sat[j]-1, f] = 1 else: Ri[nv] = varerr(nav, el[i], f) Rj[nv] = varerr(nav, el[j], f) nb[b] += 1 nv += 1 b += 1 v = np.resize(v, nv) H = np.resize(H, (nv, nav.nx)) R = ddcov(nb, b, Ri, Rj, nv) return v, H, Rconvert to matlab. write simple as possible
- Assistant
function [v, H, R] = ddres(nav, x, y, e, sat, el) % DD phase/code residual _c = 299792458; % Speed of light in m/s nf = nav.nf; ns = length(el); mode = 1; % 0:DD, 1:SD. Always use SD mode nb = zeros(2length(nav.gnss_t)nf, 1); Ri = zeros(nsnf2, 1); Rj = zeros(nsnf2, 1);
nv = 0; b = 0; H = zeros(nsnf2, nav.nx); v = zeros(nsnf2, 1);
for i = 1:length(nav.gnss_t) sys = nav.gnss_t(i); for j = 1:nf idx_f(j) = nav.obs_idx(j, sys); end
for f = 1:nf*2 if f <= nf freq = nav.freq(idx_f(f)); end % Reference satellite idx = find(sat == sys); if ~isempty(idx) [~, ind] = max(el(idx)); i = idx(ind); end for j = idx if i == j continue; end if y(i, f) == 0.0 || y(j, f) == 0.0 continue; end % DD residuals if mode == 0 if y(i+ns, f) == 0.0 || y(j+ns, f) == 0.0 continue; end v(nv+1) = (y(i, f)-y(i+ns, f))-(y(j, f)-y(j+ns, f)); else v(nv+1) = y(i, f)-y(j, f); end H(nv+1, 1:3) = -e(i, :)+e(j, :); if f <= nf % carrier idx_i = IB(sat(i), f, nav.na); idx_j = IB(sat(j), f, nav.na); lami = _c/freq; v(nv+1) = v(nv+1) - lami*(x(idx_i)-x(idx_j)); H(nv+1, idx_i) = lami; H(nv+1, idx_j) = -lami; Ri(nv+1) = varerr(nav, el(i), f); Rj(nv+1) = varerr(nav, el(j), f); nav.vsat(sat(i), f) = 1; nav.vsat(sat(j), f) = 1; else Ri(nv+1) = varerr(nav, el(i), f); Rj(nv+1) = varerr(nav, el(j), f); end nb(b+1) = nb(b+1) + 1; nv = nv + 1; end b = b + 1; endend
v = v(1:nv); H = H(1:nv, :); R = ddcov(nb, b, Ri, Rj, nv);
end