kopia lustrzana https://github.com/ag1le/morse-wip
105 wiersze
3.1 KiB
C++
105 wiersze
3.1 KiB
C++
// ----------------------------------------------------------------------------
|
|
// kalfil.c -- bayesian morse code decoder
|
|
//
|
|
// Copyright (C) 2012-2014
|
|
// (C) Mauri Niininen, AG1LE
|
|
//
|
|
// This file is part of Bayesian Morse code decoder
|
|
|
|
// bmorse is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// bmorse is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with bmorse. If not, see <http://www.gnu.org/licenses/>.
|
|
// ---------------------------------------------------------------------------
|
|
|
|
#include "bmorse.h"
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
|
|
|
|
real morse::kalfil_(real z, integer ip, real rn, integer ixs, integer kelem, integer jnode, integer israte, real dur, integer ilrate, real pin)
|
|
{
|
|
/* Initialized data */
|
|
|
|
// static real pinmin = 1e-4f;
|
|
|
|
|
|
/* System generated locals */
|
|
real r1;
|
|
|
|
|
|
/* Local variables */
|
|
static real a, g, qa, hz, pz, zr, phi, pkk, ykk, pest;
|
|
static real ppred, ypred, pzinv;
|
|
|
|
|
|
/* THIS SUBROUTINE COMPUTES THE ARRAY OF KALMAN FILTER */
|
|
/* RECURSIONS USED TO DETERMINE THE LIKELIHOODS. */
|
|
|
|
/* VARIABLES: */
|
|
/* Z - INPUT MEASUREMENT */
|
|
/* IP - INPUT PATH IDENTITY */
|
|
/* RN - INPUT NOISE POWER ESTIMATE */
|
|
/* ILX - INPUT SAVED KEYSTATE ON PATH IP */
|
|
/* IXS - INPUT KEYSTAT OF NEW NODE */
|
|
/* KELEM - INPUT ELEM STATE OF NEW NODE */
|
|
/* ISRATE INPUT SPEED STATE OF NEW NODE */
|
|
/* DUR - INPUT CURRENT DURATION OF ELEMENT ON IP */
|
|
/* ILRATE INPUT SPEED STATE ON PATH IP */
|
|
/* PIN - TRANS PROB FROM PATH IP TO NODE N */
|
|
/* LKHDJ - OUTPUT CALCULATED LIKELIHOOD VALUE */
|
|
|
|
/* SUBROUTINES USED */
|
|
/* MODEL - OBTAINS THE SIGNAL-STATE-DEPENDENT LINEAR */
|
|
/* MODEL FOR THE KALMAN FILTER RECURSIONS */
|
|
|
|
/* IF TRANSITION PROBABILITY IS VERY SMALL, DON'T */
|
|
/* BOTHER WITH LIKELIHOOD CALCULATION: */
|
|
|
|
if (pin <= 1e-4f) { // was 1e-4f
|
|
return 0.f;
|
|
}
|
|
|
|
/* OBTAIN STATE-DEPENDENT MODEL PARAMETERS: */
|
|
model_(dur, kelem, ilrate, ixs, &phi, &qa);
|
|
|
|
/* COMPUTE MEASUREMENT COEFFICIENT: */
|
|
hz = (real)ixs;
|
|
|
|
/* GET PREVIOUS ESTIMATES FOR PATH IP */
|
|
|
|
ykk = ykkip[ip];
|
|
pkk = pkkip[ip];
|
|
|
|
/* IMPLEMENT KALMAN FILTER FOR THIS TRANSITION */
|
|
|
|
ypred = phi * ykk;
|
|
ppred = phi * pkk * phi + qa;
|
|
pz = hz * ppred + rn;
|
|
pzinv = 1.f / pz;
|
|
g = ppred * hz * pzinv;
|
|
pest = (1.f - g * hz) * ppred;
|
|
zr = z - hz * ypred;
|
|
|
|
ykksv[jnode - 1] = ypred + g * zr;
|
|
pkksv[jnode - 1] = pest;
|
|
if (ykksv[jnode - 1] <= .01f) {
|
|
ykksv[jnode - 1] = .01f;
|
|
}
|
|
/* Computing 2nd power */
|
|
a = .5f*pzinv*(zr * zr);
|
|
if (a > 1e3f) {
|
|
return 0.f;
|
|
}
|
|
return (1.f / sqrt(pz)) * exp(-a);
|
|
} /* kalfil_ */
|
|
|