' kalman_filter.pb — 1D Kalman filter simulation
#COMPILE EXE
#DIM ALL

FUNCTION PBMAIN() AS LONG
    LOCAL trueValue AS DOUBLE
    LOCAL measuredValue AS DOUBLE
    LOCAL estimate AS DOUBLE
    LOCAL errorCov AS DOUBLE
    LOCAL processNoise AS DOUBLE
    LOCAL measurementNoise AS DOUBLE
    LOCAL kalmanGain AS DOUBLE
    LOCAL predictedEstimate AS DOUBLE
    LOCAL predictedErrorCov AS DOUBLE
    LOCAL innovation AS DOUBLE
    LOCAL innovationCov AS DOUBLE
    LOCAL i AS LONG
    LOCAL j AS LONG
    LOCAL numSteps AS LONG
    LOCAL estimates(200) AS DOUBLE
    LOCAL measurements(200) AS DOUBLE
    LOCAL trueValues(200) AS DOUBLE
    LOCAL totalEstError AS DOUBLE
    LOCAL totalMeasError AS DOUBLE
    LOCAL mseEst AS DOUBLE
    LOCAL mseMeas AS DOUBLE
    LOCAL iter AS LONG
    LOCAL stepIdx AS LONG
    LOCAL noiseVal AS DOUBLE
    LOCAL diffVal AS DOUBLE

    trueValue = 50.0
    estimate = 40.0
    errorCov = 100.0
    processNoise = 0.5
    measurementNoise = 10.0
    numSteps = 50

    PRINT "=== 1D Kalman Filter ==="
    PRINT "Process noise: " + STR$(processNoise)
    PRINT "Measurement noise: " + STR$(measurementNoise)

    FOR i = 0 TO numSteps - 1
        trueValue = trueValue + 0.5 + (i MOD 5) * 0.1
        trueValues(i) = trueValue

        noiseVal = (i MOD 7) * 2 - 6
        measuredValue = trueValue + noiseVal
        measurements(i) = measuredValue

        predictedEstimate = estimate
        predictedErrorCov = errorCov + processNoise

        innovation = measuredValue - predictedEstimate
        innovationCov = predictedErrorCov + measurementNoise

        kalmanGain = predictedErrorCov / innovationCov

        estimate = predictedEstimate + kalmanGain * innovation
        errorCov = (1.0 - kalmanGain) * predictedErrorCov

        estimates(i) = estimate

        IF i / 5 * 5 = i THEN
            PRINT "  Step " + STR$(i) + ": true=" + STR$(trueValue) + " meas=" + STR$(measuredValue) + " est=" + STR$(estimate)
        END IF
    NEXT i

    totalEstError = 0
    totalMeasError = 0
    FOR i = 0 TO numSteps - 1
        diffVal = estimates(i) - trueValues(i)
        IF diffVal < 0 THEN diffVal = 0 - diffVal
        totalEstError = totalEstError + diffVal * diffVal

        diffVal = measurements(i) - trueValues(i)
        IF diffVal < 0 THEN diffVal = 0 - diffVal
        totalMeasError = totalMeasError + diffVal * diffVal
    NEXT i

    mseEst = totalEstError / numSteps
    mseMeas = totalMeasError / numSteps

    PRINT "--- Results ---"
    PRINT "MSE (estimate): " + STR$(mseEst)
    PRINT "MSE (measurement): " + STR$(mseMeas)
    PRINT "Improvement: " + STR$((mseMeas - mseEst) * 100 / mseMeas) + "%"
    PRINT "=== End Kalman ==="
    FUNCTION = 0
END FUNCTION
