From cdcbd692b980bf0f518e095dd37c0281c7b5a04b Mon Sep 17 00:00:00 2001
From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Sat, 8 Aug 2015 22:29:03 +0000
Subject: [PATCH] first pass at lspmel VQ for 700b, still needs work; new BPF
 for 700b; new switches for c2sim to support 700b; removed unused quantisers
 from makefile

git-svn-id: https://svn.code.sf.net/p/freetel/code@2263 01035d8c-6547-0410-b346-abe4f91aad63
---
 codec2-dev/octave/bpf.m               |   4 +-
 codec2-dev/octave/melvq.m             |   6 +-
 codec2-dev/octave/tcohpsk.m           |   2 +-
 codec2-dev/src/CMakeLists.txt         |  83 ++++----------------
 codec2-dev/src/bpfb.h                 | 105 ++++++++++++++++++++++++++
 codec2-dev/src/c2sim.c                |  45 ++++++++---
 codec2-dev/src/codebook/lspmelvq1.txt |  65 ++++++++++++++++
 codec2-dev/src/codebook/lspmelvq2.txt |  65 ++++++++++++++++
 codec2-dev/src/codebook/lspmelvq3.txt |  65 ++++++++++++++++
 codec2-dev/src/codec2.c               |   2 +-
 codec2-dev/src/defines.h              |   1 +
 codec2-dev/src/quantise.c             |  43 +++++++++++
 codec2-dev/src/quantise.h             |   1 +
 13 files changed, 403 insertions(+), 84 deletions(-)
 create mode 100644 codec2-dev/src/bpfb.h
 create mode 100644 codec2-dev/src/codebook/lspmelvq1.txt
 create mode 100644 codec2-dev/src/codebook/lspmelvq2.txt
 create mode 100644 codec2-dev/src/codebook/lspmelvq3.txt

diff --git a/codec2-dev/octave/bpf.m b/codec2-dev/octave/bpf.m
index 5649a5c6..9e0af8fb 100644
--- a/codec2-dev/octave/bpf.m
+++ b/codec2-dev/octave/bpf.m
@@ -23,9 +23,9 @@ function write_c_array(filename, arrayname, vec)
   fclose(f);
 endfunction
 
-b=firls(100,[0 400 500 2400 2600 4000]/4000,[0.01 0.01 1 1 0.01 0.01]);
+b=firls(100,[0 250 300 2600 2700 4000]/4000,[0.01 0.01 1 1 0.01 0.01]);
 freqz(b)
-write_c_array("../src/bpf.h", "bpf", b)
+write_c_array("../src/bpfb.h", "bpfb", b)
 
 % C header file of noise samples so C version gives extacly the same results
 
diff --git a/codec2-dev/octave/melvq.m b/codec2-dev/octave/melvq.m
index 7d5c47bd..1de92b78 100644
--- a/codec2-dev/octave/melvq.m
+++ b/codec2-dev/octave/melvq.m
@@ -152,7 +152,11 @@ function ind = test_run(samplename)
   fclose(fmel_);
 end
 
-ind = test_run("all")
+ind = test_run("cq_ref");
+
+%load "../build_linux/src/all_mel.txt"
+%vq = trainvq(all_mel, 64, 3);
+%save vq;
 
 % [X] save text file of "vq quantised mels"
 % [X] load back into c2sim at run time
diff --git a/codec2-dev/octave/tcohpsk.m b/codec2-dev/octave/tcohpsk.m
index dd64c9de..e6c2db24 100644
--- a/codec2-dev/octave/tcohpsk.m
+++ b/codec2-dev/octave/tcohpsk.m
@@ -153,7 +153,7 @@ Fcentre = afdmdv.Fcentre = 1500;
 afdmdv.Fsep = afdmdv.Rs*(1+excess_bw);
 afdmdv.phase_tx = ones(afdmdv.Nc+1,1);
 % non linear carrier spacing, combined with clip, helps PAPR a lot!
-freq_hz = afdmdv.Fsep*( -Nc*Nd/2 - 0.5 + (1:Nc*Nd).^0.98 ); 
+freq_hz = afdmdv.Fsep*( -Nc*Nd/2 - 0.5 + (1:Nc*Nd).^0.98 )
 afdmdv.freq_pol = 2*pi*freq_hz/Fs;
 afdmdv.freq = exp(j*afdmdv.freq_pol);
 afdmdv.Fcentre = 1500;
diff --git a/codec2-dev/src/CMakeLists.txt b/codec2-dev/src/CMakeLists.txt
index 89e228fc..3827fa82 100644
--- a/codec2-dev/src/CMakeLists.txt
+++ b/codec2-dev/src/CMakeLists.txt
@@ -34,50 +34,12 @@ set(CODEBOOKSD
     ${D}/dlsp10.txt
 )
 
-# lspd VQ quantisers
-
-set(CODEBOOKSVQ
-    ${D}/lsp1.txt
-    ${D}/lsp2.txt
-    ${D}/lsp3.txt
-    ${D}/lsp4.txt
-    ${CMAKE_SOURCE_DIR}/unittest/lsp45678910.txt
-)
-
-set(CODEBOOKSJND
-    ${D}/lsp1.txt
-    ${D}/lsp2.txt
-    ${D}/lsp3.txt
-    ${D}/lsp4.txt
-    ${CMAKE_SOURCE_DIR}/unittest/lspjnd5-10.txt
-)
-
-set(CODEBOOKSDT
-    ${D}/lspdt1.txt
-    ${D}/lspdt2.txt
-    ${D}/lspdt3.txt
-    ${D}/lspdt4.txt
-    ${D}/lspdt5.txt
-    ${D}/lspdt6.txt
-    ${D}/lspdt7.txt
-    ${D}/lspdt8.txt
-    ${D}/lspdt9.txt
-    ${D}/lspdt10.txt
-)
-
 set(CODEBOOKSJVM
     ${D}/lspjvm1.txt
     ${D}/lspjvm2.txt
     ${D}/lspjvm3.txt
 )
 
-set(CODEBOOKSVQANSSI
-    ${D}/lspvqanssi1.txt
-    ${D}/lspvqanssi2.txt
-    ${D}/lspvqanssi3.txt
-    ${D}/lspvqanssi4.txt
-)
-
 set(CODEBOOKSMEL
     ${D}/mel1.txt
     ${D}/mel2.txt
@@ -87,6 +49,12 @@ set(CODEBOOKSMEL
     ${D}/mel6.txt
 )
 
+set(CODEBOOKSLSPMELVQ
+    ${D}/lspmelvq1.txt
+    ${D}/lspmelvq2.txt
+    ${D}/lspmelvq3.txt
+)
+
 set(CODEBOOKSGE ${D}/gecb.txt)
 
 # when crosscompiling we need a native executable
@@ -129,27 +97,6 @@ add_custom_command(
     DEPENDS generate_codebook ${CODEBOOKSD}
 )
 
-# codebookdt.c
-add_custom_command(
-    OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebookdt.c
-    COMMAND generate_codebook lsp_cbdt ${CODEBOOKSDT} > ${CMAKE_CURRENT_BINARY_DIR}/codebookdt.c
-    DEPENDS generate_codebook ${CODEBOOKSDT}
-)
-
-# codebookvq.c
-add_custom_command(
-    OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebookvq.c
-    COMMAND generate_codebook lsp_cbvq ${CODEBOOKSVQ} > ${CMAKE_CURRENT_BINARY_DIR}/codebookvq.c
-    DEPENDS generate_codebook ${CODEBOOKSVQ}
-)
-
-# codebookjnd.c
-add_custom_command(
-    OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebookjnd.c
-    COMMAND generate_codebook lsp_cbjnd ${CODEBOOKSJND} > ${CMAKE_CURRENT_BINARY_DIR}/codebookjnd.c
-    DEPENDS generate_codebook ${CODEBOOKSJND}
-)
-
 # codebookjvm.c
 add_custom_command(
     OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebookjvm.c
@@ -157,12 +104,6 @@ add_custom_command(
     DEPENDS generate_codebook ${CODEBOOKSJVM}
 )
 
-# codebookvqanssi.c
-add_custom_command(
-    OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebookvqanssi.c
-    COMMAND generate_codebook lsp_cbvqanssi ${CODEBOOKSVQANSSI} > ${CMAKE_CURRENT_BINARY_DIR}/codebookvqanssi.c
-    DEPENDS generate_codebook ${CODEBOOKSVQANSSI}
-)
 
 # codebookmel.c
 add_custom_command(
@@ -171,6 +112,13 @@ add_custom_command(
     DEPENDS generate_codebook ${CODEBOOKSMEL}
 )
 
+# codebooklspmelvq.c
+add_custom_command(
+    OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebooklspmelvq.c
+    COMMAND generate_codebook lspmelvq_cb ${CODEBOOKSLSPMELVQ} > ${CMAKE_CURRENT_BINARY_DIR}/codebooklspmelvq.c
+    DEPENDS generate_codebook ${CODEBOOKSLSPMELVQ}
+)
+
 # codebookge.c
 add_custom_command(
     OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/codebookge.c
@@ -202,12 +150,9 @@ set(CODEC2_SRCS
     pack.c
     codebook.c
     codebookd.c
-    codebookvq.c
-    codebookjnd.c
     codebookjvm.c
-    codebookvqanssi.c
-    codebookdt.c
     codebookmel.c
+    codebooklspmelvq.c
     codebookge.c
     golay23.c
     freedv_api.c
diff --git a/codec2-dev/src/bpfb.h b/codec2-dev/src/bpfb.h
new file mode 100644
index 00000000..73b1a691
--- /dev/null
+++ b/codec2-dev/src/bpfb.h
@@ -0,0 +1,105 @@
+#define BPFB_N 101
+
+float bpfb[]={
+  0.003795,
+  0.006827,
+  0.002261,
+  0.002523,
+  0.005758,
+  -0.000264,
+  -0.000674,
+  0.003113,
+  -0.004144,
+  -0.004923,
+  0.000043,
+  -0.008017,
+  -0.008711,
+  -0.001802,
+  -0.010210,
+  -0.010428,
+  -0.000899,
+  -0.009413,
+  -0.009072,
+  0.003469,
+  -0.005335,
+  -0.004828,
+  0.010724,
+  0.000941,
+  0.000708,
+  0.018957,
+  0.007084,
+  0.004825,
+  0.025418,
+  0.010147,
+  0.004452,
+  0.027434,
+  0.007550,
+  -0.002861,
+  0.023483,
+  -0.001944,
+  -0.018138,
+  0.014122,
+  -0.017583,
+  -0.040768,
+  0.002598,
+  -0.036604,
+  -0.069541,
+  -0.004273,
+  -0.054876,
+  -0.107289,
+  0.010068,
+  -0.068052,
+  -0.200119,
+  0.207287,
+  0.597150,
+  0.207287,
+  -0.200119,
+  -0.068052,
+  0.010068,
+  -0.107289,
+  -0.054876,
+  -0.004273,
+  -0.069541,
+  -0.036604,
+  0.002598,
+  -0.040768,
+  -0.017583,
+  0.014122,
+  -0.018138,
+  -0.001944,
+  0.023483,
+  -0.002861,
+  0.007550,
+  0.027434,
+  0.004452,
+  0.010147,
+  0.025418,
+  0.004825,
+  0.007084,
+  0.018957,
+  0.000708,
+  0.000941,
+  0.010724,
+  -0.004828,
+  -0.005335,
+  0.003469,
+  -0.009072,
+  -0.009413,
+  -0.000899,
+  -0.010428,
+  -0.010210,
+  -0.001802,
+  -0.008711,
+  -0.008017,
+  0.000043,
+  -0.004923,
+  -0.004144,
+  0.003113,
+  -0.000674,
+  -0.000264,
+  0.005758,
+  0.002523,
+  0.002261,
+  0.006827,
+  0.003795
+};
\ No newline at end of file
diff --git a/codec2-dev/src/c2sim.c b/codec2-dev/src/c2sim.c
index 814a0306..15e37fc6 100644
--- a/codec2-dev/src/c2sim.c
+++ b/codec2-dev/src/c2sim.c
@@ -49,6 +49,7 @@
 #include "ampexp.h"
 #include "phaseexp.h"
 #include "bpf.h"
+#include "bpfb.h"
 
 void synth_one_frame(kiss_fft_cfg fft_inv_cfg, short buf[], MODEL *model, float Sn_[], float Pn[], int prede, float *de_mem, float gain);
 void print_help(const struct option *long_options, int num_opts, char* argv[]);
@@ -66,6 +67,7 @@ int main(int argc, char *argv[])
     FILE *fin;		/* input speech file                     */
     short buf[N];	/* input/output buffer                   */
     float buf_float[N];
+    float buf_float_bpf[N];
     float Sn[M];	/* float input speech samples            */
     float Sn_pre[N];	/* pre-emphasised input speech samples   */
     COMP  Sw[FFT_ENC];	/* DFT of Sn[]                           */
@@ -89,7 +91,7 @@ int main(int argc, char *argv[])
     int lpc_model = 0, order = LPC_ORD;
     int lsp = 0, lspd = 0, lspvq = 0;
     int lspres = 0;
-    int lspjvm = 0, lspjnd = 0, lspmel = 0;
+    int lspjvm = 0, lspjnd = 0, lspmel = 0, lspmelvq = 0;
     #ifdef __EXPERIMENTAL__
     int lspanssi = 0, 
     #endif
@@ -103,6 +105,7 @@ int main(int argc, char *argv[])
     float ex_phase[MAX_AMP+1];
 
     int   postfilt;
+    float bg_est = 0.0;
 
     int   hand_voicing = 0, phaseexp = 0, ampexp = 0, hi = 0, simlpcpf = 0, lspmelread = 0;
     int   lpcpf = 0;
@@ -135,6 +138,7 @@ int main(int argc, char *argv[])
     struct AEXP *aexp = NULL;
     float gain = 1.0;
     int   bpf_en = 0;
+    int   bpfb_en = 0;
     float bpf_buf[BPF_N+N];
 
     char* opt_string = "ho:";
@@ -143,6 +147,7 @@ int main(int argc, char *argv[])
         { "lspjnd", no_argument, &lspjnd, 1 },
         { "lspmel", no_argument, &lspmel, 1 },
         { "lspmelread", required_argument, &lspmelread, 1 },
+        { "lspmelvq", required_argument, &lspmelvq, 1 },
         { "lsp", no_argument, &lsp, 1 },
         { "lspd", no_argument, &lspd, 1 },
         { "lspvq", no_argument, &lspvq, 1 },
@@ -167,6 +172,7 @@ int main(int argc, char *argv[])
         { "rate", required_argument, NULL, 0 },
         { "gain", required_argument, NULL, 0 },
         { "bpf", no_argument, &bpf_en, 1 },
+        { "bpfb", no_argument, &bpfb_en, 1 },
         #ifdef DUMP
         { "dump", required_argument, &dump, 1 },
         #endif
@@ -373,6 +379,8 @@ int main(int argc, char *argv[])
     if (ampexp)
 	aexp = amp_experiment_create();
 
+    if (bpfb_en)
+        bpf_en = 1;
     if (bpf_en) {
         for(i=0; i<BPF_N; i++)
             bpf_buf[i] = 0.0;
@@ -411,20 +419,30 @@ int main(int argc, char *argv[])
         }
          
         if (bpf_en) {
+            /* filter input speech to create buf_float_bpf[], this is fed to the 
+               LPC modelling.  Unfiltered speech in in buf_float[], which is
+               delayed to match that of the BPF */
+
+            /* BPF speech */
+
             for(i=0; i<BPF_N; i++)
                 bpf_buf[i] =  bpf_buf[N+i];
             for(i=0; i<N; i++)
                 bpf_buf[BPF_N+i] = buf_float[i];
-            inverse_filter(&bpf_buf[BPF_N], bpf, N, buf_float, BPF_N);
-       }
+            if (bpfb_en)
+                inverse_filter(&bpf_buf[BPF_N], bpfb, N, buf_float, BPF_N);
+            else
+                inverse_filter(&bpf_buf[BPF_N], bpf, N, buf_float, BPF_N);
+        }
 
         /* shift buffer of input samples, and insert new samples */
 
 	for(i=0; i<M-N; i++) {
 	    Sn[i] = Sn[i+N];
 	}
-	for(i=0; i<N; i++)
+	for(i=0; i<N; i++) {
 	    Sn[i+M-N] = buf_float[i];
+        }
 
 	/*------------------------------------------------------------*\
 
@@ -505,7 +523,7 @@ int main(int argc, char *argv[])
 
 	    for(i=0; i<=MAX_AMP; i++)
 	    	model.phi[i] = 0;
-	
+	    
             /* Determine DFT of A(exp(jw)), which is needed for phase0 model when
                LPC is not used, e.g. indecimate=1 (10ms) frames with no LPC */
 
@@ -649,11 +667,15 @@ int main(int argc, char *argv[])
                     assert(ret == order);
                 }
                                 
+                if (lspmelvq) {
+                    lspmelvq_quantise(mel, mel, 6);
+                }
+
                 /* ensure no unstable filters after quantisation */
 
-                #define MEL_ROUND 50
+                #define MEL_ROUND 10
 		for(i=1; i<order; i++) {
-		    if (mel[i] <= mel[i-1]) {
+		    if (mel[i] <= mel[i-1]+MEL_ROUND) {
 			mel[i]+=MEL_ROUND/2;
 			mel[i-1]-=MEL_ROUND/2;
                         i = 1;
@@ -738,6 +760,8 @@ int main(int argc, char *argv[])
 
                 if (phase0)
                     phase_synth_zero_order(fft_fwd_cfg, &model_dec[i], ex_phase, Aw);	
+                if (postfilt)
+                    postfilter(&model_dec[i], &bg_est);
                 synth_one_frame(fft_inv_cfg, buf, &model_dec[i], Sn_, Pn, prede, &de_mem, gain);
                 if (fout != NULL) fwrite(buf,sizeof(short),N,fout);
             }
@@ -785,11 +809,12 @@ int main(int argc, char *argv[])
 	fclose(fvoicing);
 
     nlp_destroy(nlp_states);
-
+    
     return 0;
 }
-
-void synth_one_frame(kiss_fft_cfg fft_inv_cfg, short buf[], MODEL *model, float Sn_[], float Pn[], int prede, float *de_mem, float gain)
+                
+void synth_one_frame(kiss_fft_cfg fft_inv_cfg, short buf[], MODEL *model, float Sn_[], 
+                     float Pn[], int prede, float *de_mem, float gain)
 {
     int     i;
 
diff --git a/codec2-dev/src/codebook/lspmelvq1.txt b/codec2-dev/src/codebook/lspmelvq1.txt
new file mode 100644
index 00000000..e13a3302
--- /dev/null
+++ b/codec2-dev/src/codebook/lspmelvq1.txt
@@ -0,0 +1,65 @@
+ 6 64
+ 6.40185185e+02 8.93138889e+02 1.39385185e+03 1.49406481e+03 1.65625926e+03 1.70082407e+03
+ 5.58515625e+02 7.16906250e+02 1.05735938e+03 1.18262500e+03 1.50379688e+03 1.61003125e+03
+ 6.15108108e+02 7.69621622e+02 1.12016216e+03 1.22283784e+03 1.36645946e+03 1.46505405e+03
+ 6.63458333e+02 7.36833333e+02 9.64763889e+02 1.04354167e+03 1.62316667e+03 1.68129167e+03
+ 4.87957447e+02 5.77723404e+02 1.01742553e+03 1.17704255e+03 1.55209574e+03 1.61569149e+03
+ 5.36098765e+02 7.33407407e+02 1.29228395e+03 1.40608642e+03 1.57770370e+03 1.63749383e+03
+ 4.73014706e+02 5.42558824e+02 8.77397059e+02 1.28582353e+03 1.59104412e+03 1.64744118e+03
+ 5.25342857e+02 6.52014286e+02 1.20682857e+03 1.49395714e+03 1.64797143e+03 1.69830000e+03
+ 5.10886792e+02 5.72867925e+02 9.45226415e+02 1.44567925e+03 1.67816981e+03 1.70552830e+03
+ 5.34914530e+02 7.21264957e+02 1.27592308e+03 1.41576068e+03 1.64850427e+03 1.69573504e+03
+ 8.65189189e+02 1.04700000e+03 1.26713514e+03 1.38932432e+03 1.64656757e+03 1.69697297e+03
+ 6.08033113e+02 8.69887417e+02 1.30094702e+03 1.43286755e+03 1.63974172e+03 1.68941060e+03
+ 5.54971831e+02 6.49352113e+02 8.66845070e+02 9.79873239e+02 1.64530986e+03 1.69539437e+03
+ 6.96079208e+02 8.13970297e+02 1.10248515e+03 1.21979208e+03 1.53648515e+03 1.62194059e+03
+ 5.53878788e+02 6.91096970e+02 1.20083636e+03 1.33933939e+03 1.62908485e+03 1.68349697e+03
+ 7.78561224e+02 9.97775510e+02 1.25863265e+03 1.39033673e+03 1.60198980e+03 1.65785714e+03
+ 7.13107143e+02 7.78892857e+02 9.92875000e+02 1.05194643e+03 1.49744643e+03 1.65066071e+03
+ 4.90269663e+02 5.98179775e+02 1.11602247e+03 1.24413483e+03 1.62225843e+03 1.67221348e+03
+ 4.48555556e+02 5.12085470e+02 1.27170085e+03 1.44817949e+03 1.57936752e+03 1.64247863e+03
+ 4.65687500e+02 5.35312500e+02 1.09918750e+03 1.53579167e+03 1.68429167e+03 1.71089583e+03
+ 8.12222222e+02 1.08753333e+03 1.47044444e+03 1.55973333e+03 1.69217778e+03 1.72637778e+03
+ 4.28173913e+02 4.89426087e+02 1.16033043e+03 1.40930435e+03 1.59750435e+03 1.65129565e+03
+ 7.12810000e+02 9.57560000e+02 1.43302000e+03 1.51637000e+03 1.67539000e+03 1.71006000e+03
+ 7.17255474e+02 9.34072993e+02 1.30520438e+03 1.43671533e+03 1.64720438e+03 1.69308759e+03
+ 4.92887640e+02 5.80393258e+02 1.33951685e+03 1.46106742e+03 1.59200000e+03 1.65341573e+03
+ 5.50467290e+02 6.75887850e+02 9.90887850e+02 1.17743925e+03 1.61563551e+03 1.65800000e+03
+ 7.14528302e+02 8.01792453e+02 1.07294340e+03 1.14607547e+03 1.63705660e+03 1.70658491e+03
+ 5.13819149e+02 5.90989362e+02 7.98691489e+02 8.95755319e+02 1.55775532e+03 1.62456383e+03
+ 4.36653333e+02 5.18413333e+02 1.28198667e+03 1.54708000e+03 1.68512000e+03 1.71776000e+03
+ 6.81854167e+02 7.58354167e+02 1.04664583e+03 1.12091667e+03 1.41225000e+03 1.60341667e+03
+ 8.73961538e+02 1.11849038e+03 1.37660577e+03 1.46506731e+03 1.66537500e+03 1.70718269e+03
+ 5.53528736e+02 6.34091954e+02 1.14477011e+03 1.28480460e+03 1.54287356e+03 1.62063218e+03
+ 4.48532258e+02 5.19096774e+02 1.05457258e+03 1.31966129e+03 1.59125806e+03 1.64984677e+03
+ 7.42266667e+02 8.85293333e+02 1.15282667e+03 1.31850667e+03 1.56936000e+03 1.63145333e+03
+ 5.29029851e+02 6.54522388e+02 1.35576119e+03 1.51174627e+03 1.66238806e+03 1.70640299e+03
+ 4.63793651e+02 5.97769841e+02 1.17604762e+03 1.36613492e+03 1.62936508e+03 1.67800794e+03
+ 6.26936170e+02 7.06659574e+02 1.05804255e+03 1.32361702e+03 1.47327660e+03 1.59968085e+03
+ 4.77322222e+02 6.15500000e+02 1.48888889e+03 1.55050000e+03 1.68310000e+03 1.71234444e+03
+ 5.47441558e+02 8.15441558e+02 1.31337662e+03 1.48696104e+03 1.67197403e+03 1.71740260e+03
+ 6.10670968e+02 8.19954839e+02 1.21910968e+03 1.36365806e+03 1.59205161e+03 1.65430968e+03
+ 5.47413793e+02 7.46540230e+02 1.43842529e+03 1.51772414e+03 1.65964368e+03 1.69557471e+03
+ 6.04822917e+02 8.21145833e+02 1.13793750e+03 1.35829167e+03 1.59893750e+03 1.65563542e+03
+ 5.25934783e+02 6.16739130e+02 1.06013043e+03 1.42732609e+03 1.59334783e+03 1.65747826e+03
+ 6.22500000e+02 7.62142857e+02 1.31865306e+03 1.41095918e+03 1.61812245e+03 1.68006122e+03
+ 4.36916667e+02 5.16583333e+02 1.39028571e+03 1.47585714e+03 1.59471429e+03 1.63373810e+03
+ 7.92486726e+02 1.03123894e+03 1.36261947e+03 1.47268142e+03 1.64925664e+03 1.69735398e+03
+ 4.57706897e+02 5.26206897e+02 8.65965517e+02 1.12046552e+03 1.56482759e+03 1.62527586e+03
+ 5.26390000e+02 6.24210000e+02 1.26965000e+03 1.37423000e+03 1.55807000e+03 1.62096000e+03
+ 4.83768421e+02 5.73505263e+02 1.44066316e+03 1.51243158e+03 1.62236842e+03 1.67130526e+03
+ 9.53060606e+02 1.19403030e+03 1.41666667e+03 1.51581818e+03 1.67881818e+03 1.71821212e+03
+ 4.99947368e+02 6.27357895e+02 1.29993684e+03 1.39423158e+03 1.64316842e+03 1.68532632e+03
+ 6.48722892e+02 8.38180723e+02 1.22550000e+03 1.38344578e+03 1.63746386e+03 1.69167470e+03
+ 6.72588235e+02 1.02260294e+03 1.34620588e+03 1.44375000e+03 1.65114706e+03 1.69542647e+03
+ 5.81833333e+02 6.74944444e+02 9.55166667e+02 1.02050000e+03 1.37050000e+03 1.50311111e+03
+ 5.36142857e+02 6.52530612e+02 1.24383673e+03 1.31526531e+03 1.42510204e+03 1.50573469e+03
+ 5.60558376e+02 7.86649746e+02 1.22465990e+03 1.37398477e+03 1.63005584e+03 1.68268020e+03
+ 5.91925926e+02 7.83722222e+02 9.82000000e+02 1.14081481e+03 1.58161111e+03 1.62527778e+03
+ 5.48536585e+02 6.44524390e+02 9.40451220e+02 1.04874390e+03 1.55789024e+03 1.60947561e+03
+ 5.16915789e+02 7.23252632e+02 1.13595263e+03 1.30918947e+03 1.60008421e+03 1.66947895e+03
+ 4.26521127e+02 5.06077465e+02 1.45773239e+03 1.53502113e+03 1.64134507e+03 1.67845070e+03
+ 6.45189189e+02 7.76594595e+02 1.02078378e+03 1.24006757e+03 1.59700000e+03 1.64870270e+03
+ 6.37105263e+02 9.41473684e+02 1.24221053e+03 1.37248684e+03 1.64669737e+03 1.69480263e+03
+ 6.91228261e+02 7.88141304e+02 1.20211957e+03 1.29489130e+03 1.62696739e+03 1.68177174e+03
+ 6.99080460e+02 8.86655172e+02 1.30089655e+03 1.39927586e+03 1.57994253e+03 1.64679310e+03
diff --git a/codec2-dev/src/codebook/lspmelvq2.txt b/codec2-dev/src/codebook/lspmelvq2.txt
new file mode 100644
index 00000000..fd866c27
--- /dev/null
+++ b/codec2-dev/src/codebook/lspmelvq2.txt
@@ -0,0 +1,65 @@
+ 6 64
+ 4.44342023e+00 5.11708330e+01 7.45725570e+00 -2.74373415e+01 -1.85055703e+01 -1.81988855e+01
+ 5.15799536e+01 6.33165865e+01 5.77795877e+01 -4.41590891e+01 -1.84834304e+01 -6.93391834e+00
+ -2.02794858e+01 -2.17454062e+01 4.66946917e+00 5.21568589e+01 3.04367068e+01 3.68582243e+01
+ -2.91103567e+01 -5.63932590e+00 -3.45382916e+00 -6.30261110e+01 -2.04423477e+01 -1.90484888e+01
+ 2.91622389e+00 4.08374256e+01 1.65790036e+01 -5.18460849e+01 3.85044571e+01 1.81728298e+01
+ -2.06976591e+01 -1.14022219e+01 -3.66173122e+01 -1.66116065e+01 -5.68964685e+01 -2.43009778e+01
+ -2.02385110e+01 2.66331668e+01 3.31191422e+01 2.76284155e+01 -3.67493353e+01 -2.56040969e+01
+ 5.49870513e+01 7.17479528e-01 2.30674073e+01 -2.21030578e+01 1.16642536e+01 1.09937884e+01
+ -6.27214697e+01 2.17547491e+01 2.12906874e+01 -7.64891287e+00 -4.55329824e+00 -9.71776749e+00
+ -9.56338101e+00 -3.85841094e+00 2.50453734e+01 -9.45216004e+00 6.05016771e+00 5.35043165e+00
+ 4.73822779e+01 5.66122493e+01 -2.70314508e+01 -2.46699892e+01 4.86343141e+00 -2.25494963e-01
+ 2.63996634e+01 2.67857198e+01 -1.66167044e+00 6.28366080e+01 -1.97652890e+01 -8.55169245e+00
+ -8.77648251e+00 -9.04545090e+00 -7.88995628e+00 2.82433004e+01 -3.50963028e+01 -2.19709149e+01
+ 1.48422904e+01 2.54563234e+01 -5.61262119e+01 -5.02933841e+01 -2.29392506e+01 -1.58112708e+01
+ 4.94185528e+00 2.77798011e+01 8.34578754e+00 1.08553274e+01 -3.12587321e+00 -3.97806637e+00
+ 1.26426172e+01 -5.22317245e+01 3.72487149e+01 -5.72067399e+01 -1.45125408e+01 -5.54034776e+00
+ 1.34849886e+01 1.53245571e+01 -2.39643623e+01 -2.13134975e+01 1.95779488e+01 1.41596754e+01
+ -5.53542844e+01 -4.52077225e+01 1.05184665e+01 4.30460961e+01 -2.49859454e+01 -1.93484138e+01
+ 2.77226366e+01 3.21881626e+01 2.00321485e+01 2.43327500e+01 -7.28194058e+01 -5.11822933e+01
+ -3.13817941e+01 -5.25745440e+00 -4.37806471e+01 1.41312375e+01 1.76392003e+01 9.81024019e+00
+ -4.82599528e+01 -2.62973101e+01 -4.41428355e+01 -3.19000800e+01 2.25085221e+01 -4.67937987e-01
+ 7.37202096e+00 -7.79070841e+00 -1.25731600e+01 2.71074123e+01 9.34051572e+00 1.44476918e+01
+ 1.45294918e+01 8.82597108e+00 5.70008598e+01 -1.63234076e+01 -3.24142014e+01 -2.10223676e+01
+ 3.24616251e+01 4.86062215e+01 3.85451764e+01 9.77181881e+00 1.82855803e+00 1.15062929e+01
+ -4.38275303e+01 -2.26262503e+01 -2.98278038e+01 1.36114828e+01 9.66848589e+00 -6.35218137e+01
+ -1.19967019e+01 2.74307686e+00 -7.36375365e+01 -2.09808902e+01 -4.11838983e+00 7.71405435e+00
+ 2.40161741e+01 -2.29513293e+00 -6.80983466e+00 -2.64043268e+01 -2.18529016e+01 -1.63381291e+01
+ -1.61483826e+01 3.59085531e+01 -3.08369768e+00 3.83957559e+00 4.23002878e+01 1.75002976e+01
+ 5.41225147e+01 -4.87513410e+01 -1.48712329e+01 -3.81255636e+01 1.52903471e+01 7.33079239e+00
+ 5.30928834e+01 1.39221339e+01 1.06535591e+01 2.43449656e+01 -1.65952406e+01 -1.60364654e+01
+ 3.31414865e+01 3.85713980e+01 -2.60250508e+01 2.20210341e+01 1.56865619e+01 1.32592795e+01
+ 1.57193842e+01 -4.94061178e+01 3.14552297e+01 1.01896451e+01 2.19911283e-01 1.62901547e+00
+ 4.10868090e+00 1.42755212e+01 5.84749767e+01 -1.16668460e+00 5.26264856e+01 4.34937534e+01
+ 3.58653333e+02 -1.12586667e+02 8.59866667e+01 5.20800000e+01 -5.28800000e+01 -2.49240000e+02
+ -1.25792149e+01 -6.64039481e+00 -3.30106093e+01 1.51449305e+00 5.03259445e+01 6.16091231e+01
+ -6.81685332e+00 1.43146311e+01 1.41562793e+01 5.36363349e+01 8.36051350e+01 3.81739887e+01
+ -1.80005518e+01 4.13574694e+01 -4.63736420e+01 8.47794181e+00 -1.04610694e+01 -1.13846635e+01
+ 2.87711486e+01 3.14689155e+01 -3.95743803e+01 1.43977222e+00 -3.77309497e+01 -3.05309094e+01
+ -3.44692147e+01 -1.10777698e+01 4.44680522e+01 -6.00045330e+01 -4.49729480e+01 -3.48364296e+01
+ 3.87401321e+01 4.10529042e+01 -7.92945674e+00 2.00279378e+01 7.44245854e+01 5.46497838e+01
+ 8.18008445e+01 1.33530724e+02 -5.47374752e+00 3.44759309e+01 7.57416682e+01 5.26111643e+01
+ -6.15067453e+01 -7.66327843e+01 -4.72471128e+01 -4.35927870e+01 -9.46878103e+00 8.32597962e-01
+ -1.15439769e+01 -4.47727735e+01 -1.39256628e+01 -3.32121575e+00 2.48160127e+01 2.19063760e+01
+ 2.83944855e+00 -4.50522337e+01 -3.68776005e+01 -1.47576729e+01 -1.19911863e+01 -8.75366538e+00
+ -8.97656602e+00 -1.40499000e+01 3.92627525e+01 4.88037864e+01 -1.43789189e+01 -4.79625297e+00
+ 3.16291704e+01 3.20168152e+01 5.52277932e+00 -7.98410630e+01 -2.41788753e+01 -1.57596645e+01
+ 1.58930283e+01 1.04667104e+01 -4.39057154e+01 -2.04884565e+01 8.07593937e+01 6.80229829e+00
+ 2.23284907e+01 -6.94941558e+01 -5.16156062e+00 4.88867716e+01 4.01995064e+00 -3.04376362e+00
+ 3.14062302e+01 -2.00952330e+01 -5.51259434e+01 5.05988423e-01 2.08599646e+01 1.50816325e+01
+ -1.29696080e+01 3.72548221e+01 1.81009320e+01 5.10766991e+01 1.82053491e+01 1.98473834e+01
+ -4.54854892e+01 -4.53453683e+01 -4.22794830e+00 -1.58693488e+01 -6.07271867e+00 -1.99631436e+00
+ -9.25461576e+00 4.50241444e+01 7.02878753e+01 -4.44483888e+01 2.63164730e+00 4.15336265e+00
+ -5.11588738e+01 -2.77586316e+01 1.97999488e+01 -9.87881525e+00 6.34125456e+01 4.83124190e+01
+ -8.94084134e+00 -1.34814529e+01 -4.49601913e+01 5.28390455e+01 -1.72582148e+01 -1.59438499e+00
+ 2.31799880e+01 -3.00373757e+00 3.31620321e+01 3.15423901e+01 2.91110463e+01 2.59326679e+01
+ -2.84665305e+01 -1.06868238e+01 8.87542554e+01 -3.88024366e+00 1.75880577e+01 1.37336245e+01
+ -1.06577981e+01 7.43021469e+00 -2.09450218e+01 -1.76491054e+01 -1.12815357e+01 -1.08000535e+01
+ -3.56182243e+01 -6.90300711e+00 3.92829138e+00 2.69695474e+01 2.88940235e+00 5.01684684e+00
+ -3.78262741e+00 -1.92095454e+01 -1.05567529e+01 -6.77467970e+01 2.03733175e+01 2.95520123e+01
+ 5.38108573e+01 -3.38374433e+01 -2.78223132e+01 -7.73288956e+00 -3.13462176e+01 -2.35677556e+01
+ -3.88761929e+01 -1.77679216e+01 1.14569557e+01 2.84607024e+01 5.07005660e+01 -1.92487580e+01
+ -4.46024476e+01 -6.02805570e+01 5.63500111e+01 2.17544648e+01 5.23676894e+00 7.66608298e+00
+ -3.80758080e+00 -3.91424613e+01 2.88304674e+01 -3.09285226e+00 -4.27534207e+01 -2.51802947e+01
+ 4.52558846e+01 2.88741805e+01 6.00723263e+01 6.44609745e+01 3.51202685e+00 -2.78829813e+01
diff --git a/codec2-dev/src/codebook/lspmelvq3.txt b/codec2-dev/src/codebook/lspmelvq3.txt
new file mode 100644
index 00000000..17b4d184
--- /dev/null
+++ b/codec2-dev/src/codebook/lspmelvq3.txt
@@ -0,0 +1,65 @@
+ 6 64
+ -9.63558134e+00 2.75500520e+01 1.54445352e+01 -4.34872231e+00 -1.85869537e+00 1.27053793e+00
+ -1.55342902e+01 2.30514670e+01 -1.54436233e+01 -2.01886592e+00 -6.19433520e+00 -1.35084579e+01
+ 8.38867370e+00 -1.60997829e+00 -3.23903372e+01 1.01764745e+01 1.94669519e+00 1.22453903e+01
+ -3.06305579e+00 -9.55983454e+00 1.43669912e+01 -1.87158872e+00 1.96192339e+01 -3.78366429e+00
+ -1.84949911e+01 -1.33811009e+01 -2.38928068e+01 -2.03745297e+01 -6.87856219e+00 -1.74886898e+01
+ 1.54924765e+01 1.47070114e+01 -6.97854547e-02 1.57541015e+01 1.26051117e+01 -3.19767598e+00
+ 9.60466380e+00 -4.56494120e+00 1.01616352e+01 1.55593699e+01 6.89224494e+00 -3.15602059e+01
+ -1.15624623e+01 -2.30872118e+01 3.49162756e+01 1.23051800e+01 7.67426494e+00 -1.26297575e+00
+ 2.54859847e+00 -3.90250852e+00 -1.91132460e+01 7.61309526e+00 -3.10016268e+01 1.24758542e+01
+ 2.74156140e+00 1.24123828e+01 -3.95057506e+01 -3.25024460e-01 -2.29186113e+01 -2.85606286e+01
+ 6.30148114e+00 1.54401829e+01 -2.62839880e+00 -2.01603121e+01 5.22906241e+00 -1.23450527e+01
+ -6.91861647e+00 1.66334555e+01 -1.65064009e+00 2.99602193e+00 -2.39479486e+01 -1.43946621e+00
+ -1.43906662e+01 -3.14169988e+01 1.01112600e+01 1.70013058e+00 -2.15732981e+01 4.73599588e+00
+ -1.67171131e+00 6.22750796e+00 -1.37186892e+01 2.10936412e+01 -9.69243460e+00 -1.05755505e+01
+ 2.15265759e+00 2.12197974e+01 -1.30171434e+01 -1.43134936e+00 1.88830899e+01 1.06664329e+01
+ 1.38912823e+01 2.73564952e+01 4.72837867e-01 -7.40477398e+00 -1.48704788e+01 2.57448101e+01
+ 2.84020077e+01 -2.05484249e+00 -9.32712325e+00 -1.73169289e+01 1.56430267e+01 6.96908165e+00
+ -1.58630340e+01 -1.78482175e+01 -2.49238306e+01 1.25573572e+01 7.17565614e+00 1.61971650e-02
+ 5.99290542e+00 -4.10228475e+01 1.95790603e+00 -6.78011978e+00 9.20162437e+00 4.62340214e+00
+ -6.33628710e+00 -7.61678949e+00 2.77318403e+01 -8.92139602e+00 -1.41931012e+01 7.88246832e+00
+ -1.22367255e+01 -2.12449687e+01 -2.59270048e+00 1.37776088e+01 -2.78639807e+00 -2.45071888e+01
+ 2.31499628e+01 -9.93687285e+00 -2.92558947e+00 3.88086233e+00 1.12667187e+01 1.11998192e+01
+ 9.67437083e+00 -9.42689854e+00 6.25820343e+00 3.65694017e+01 2.88654337e+00 1.82051686e+00
+ 1.53850350e+01 -2.88243395e+00 1.98376862e+01 -1.45110879e+01 5.92264164e+00 -2.55757052e+00
+ -2.09648354e+01 3.76147238e+00 1.85073811e+01 -1.35546710e+01 -7.84260856e+00 -1.99799886e+01
+ 2.46031823e+01 1.69890405e+01 -1.91622335e+01 1.35534708e+00 -1.22027387e-02 1.66227368e-01
+ 9.98886112e+00 -6.89665978e+00 -2.05111005e+01 -2.89196132e+00 2.58467075e+00 -1.70490443e+01
+ -9.17760933e+00 -2.37209261e+01 1.26087522e+01 -1.82653678e+01 2.17717972e+00 -1.30865104e+01
+ -9.73325636e+00 -1.22681927e+01 6.80913709e+00 2.03469383e+01 -1.02912141e+01 4.85191264e+00
+ -5.19405971e+00 6.78013590e+00 1.80099171e+01 1.43782234e+01 -1.24328209e-01 -1.00140628e+01
+ -4.69805841e+00 6.71393347e+00 -1.91371131e+01 8.19813751e+00 2.33986930e+01 -1.03316046e+01
+ 1.02965114e+00 -4.63654206e+00 2.19822248e+01 1.15087729e+01 -3.09616861e+01 -2.06353989e+01
+ -3.32824280e+01 3.14666327e+01 -1.14836699e+01 1.15143750e+01 -1.36833847e+00 9.44598605e+00
+ -3.75430917e+01 -3.16316521e+00 -2.09497259e+00 -2.62711963e+00 4.04714006e+01 -3.30527290e+01
+ -8.79595013e+00 -1.55173952e+01 -1.57915761e+01 7.97003454e+00 3.72542479e+01 4.07063036e+01
+ -1.47261309e+01 -1.26883966e+01 2.42105009e+00 -1.02685695e+01 2.59032576e+01 1.48524666e+01
+ 2.29597589e+01 -1.66223954e+01 -3.64948776e+00 4.44268970e+00 -2.23896609e+01 1.36968303e+01
+ -1.08740414e+01 4.18930892e+00 -2.42283549e+01 -3.63763568e+00 -1.51378968e+01 4.09515145e+01
+ 2.82393122e+01 -8.63224808e+00 -1.25439700e+01 2.88282453e+01 -9.87894477e-01 -4.98239761e+00
+ -2.50777306e+01 -4.81677621e-01 -3.37082359e+00 5.55113597e+00 -9.89897733e+00 -8.07628392e+00
+ 2.33580811e+01 3.12034460e+00 -8.63347483e+00 6.30419907e-01 -1.82215617e+01 -2.21886118e+01
+ 4.49504750e+01 1.98266721e+01 2.37129225e+01 8.58074741e+00 7.80458003e+00 1.78796190e+00
+ -8.13112376e+00 -2.12620216e+00 -7.12776152e+00 -2.50528525e+01 -1.67286912e+01 8.41402395e+00
+ 2.01964856e+00 1.92578572e+01 2.00963154e+01 5.99198987e+00 2.81097545e+01 5.96128119e+00
+ 2.42492523e+00 -6.33215684e+00 -2.65858257e+01 -2.38606757e+01 8.27049181e+00 3.05804887e+00
+ 1.53248169e-02 4.46111848e-01 -4.92759469e+00 1.90023172e+01 2.27345854e+01 1.55450838e+01
+ -7.39591162e+00 4.02849951e+01 1.08413779e+01 2.57961044e+01 -8.81069145e+00 -1.34999568e+01
+ 5.81305664e+00 1.12384032e+01 6.93765484e+00 -9.43066771e+00 9.51417968e+00 2.29708947e+01
+ -9.17611356e+00 -1.64993397e+01 -1.56928999e+00 -2.91110450e+00 4.17112939e+00 1.09227968e+01
+ 1.00375713e+01 -2.74993429e+01 -8.25331597e+00 -1.71500301e+00 -1.15063443e+01 -1.04669902e+01
+ 2.36636508e+01 -1.38337857e+01 1.47284089e+01 8.00341253e+00 -2.71880968e+00 -8.80708335e+00
+ 2.71009769e+01 2.42801325e+00 1.14599376e+01 -2.41577495e+01 -2.09900573e+01 4.52357891e+00
+ 1.68065353e+01 1.93315410e+01 1.11219486e+01 1.33391053e+01 -1.31522072e+01 9.14279911e-01
+ -2.52603434e+01 6.04836929e+00 1.21994481e+01 2.19372400e+01 1.48794817e+01 6.93368419e+00
+ -1.24639103e+00 -7.96856388e+00 1.64064238e+01 -2.36409457e+00 -2.59092550e+01 4.60937831e+01
+ 8.81686599e+00 2.48004312e+01 1.14474515e+01 -1.32610170e+01 -1.98693483e+01 -2.84792661e+01
+ 1.53175060e+01 -1.25335244e+01 2.41777946e+01 1.05132987e+01 2.22243698e+01 9.93190900e+00
+ -1.87981919e+01 3.89390253e+01 -2.00631273e+01 -3.54051693e+01 7.58790045e+00 -5.99372720e-01
+ -1.81387622e+01 9.58429879e+00 1.74375056e+01 -2.10570251e+01 8.16340142e+00 6.31215716e+00
+ -6.15485876e+01 -8.71159172e+00 1.97501971e+01 -2.52365495e+01 3.56558072e+00 -1.64973217e+00
+ -2.69862835e+01 2.91016925e-01 -1.25336894e+01 -1.05049848e+01 1.10613561e+01 4.84071822e+00
+ -6.30579095e-01 -3.87056005e+00 -7.07794677e-01 -1.31305521e+01 -1.96547570e+01 -1.62436152e+01
+ 4.87021595e+00 -5.90743926e+00 3.46971058e+00 -4.00865595e+01 1.68740955e+01 1.03332584e+01
+ -1.00985334e+00 1.62339777e+01 -4.75836148e-01 2.83847633e+01 1.75473354e+00 2.82608265e+01
diff --git a/codec2-dev/src/codec2.c b/codec2-dev/src/codec2.c
index c2eff533..8482fb87 100644
--- a/codec2-dev/src/codec2.c
+++ b/codec2-dev/src/codec2.c
@@ -1372,7 +1372,7 @@ void codec2_encode_700(struct CODEC2 *c2, unsigned char * bits, short speech[])
         c2->bpf_buf[BPF_N+i] = speech[i];
     inverse_filter(&c2->bpf_buf[BPF_N], bpf, 4*N, bpf_out, BPF_N);
     for(i=0; i<4*N; i++)
-        bpf_speech[i] = speech[i];
+        bpf_speech[i] = bpf_out[i];
 
     /* frame 1 --------------------------------------------------------*/
 
diff --git a/codec2-dev/src/defines.h b/codec2-dev/src/defines.h
index eac1ab70..5275ad7f 100644
--- a/codec2-dev/src/defines.h
+++ b/codec2-dev/src/defines.h
@@ -91,5 +91,6 @@ extern const struct lsp_codebook lsp_cbjvm[];
 extern const struct lsp_codebook lsp_cbvqanssi[];
 extern const struct lsp_codebook mel_cb[];
 extern const struct lsp_codebook ge_cb[];
+extern const struct lsp_codebook lspmelvq_cb[];
 
 #endif
diff --git a/codec2-dev/src/quantise.c b/codec2-dev/src/quantise.c
index 3e4adfd3..261a1ccc 100644
--- a/codec2-dev/src/quantise.c
+++ b/codec2-dev/src/quantise.c
@@ -136,6 +136,8 @@ long quantise(const float * cb, float vec[], float w[], int k, int m, float *se)
    return(besti);
 }
 
+
+
 /*---------------------------------------------------------------------------*\
 									      
   encode_lspds_scalar()
@@ -198,6 +200,7 @@ void encode_lspds_scalar(
 
 }
 
+
 void decode_lspds_scalar(
 		 float lsp_[], 
 		 int   indexes[],
@@ -531,6 +534,46 @@ void lspjvm_quantise(float *x, float *xq, int order)
   }
 }
 
+
+/* simple (non mbest) 6th order LSP MEL VQ quantiser */
+
+void lspmelvq_quantise(float *x, float *xq, int order)
+{
+  int i, n1, n2, n3;
+  float err[order], err2[order], err3[order];
+  float w[order], w2[order], w3[order];
+  const float *codebook1 = lspmelvq_cb[0].cb;
+  const float *codebook2 = lspmelvq_cb[1].cb;
+  const float *codebook3 = lspmelvq_cb[2].cb;
+  float tmp[order];
+
+  assert(order == lspmelvq_cb[0].k);
+
+  n1 = find_nearest(codebook1, lspmelvq_cb[0].m, x, order);
+  
+  for (i=0; i<order; i++) {
+    tmp[i] = codebook1[order*n1+i];
+    err[i] = x[i] - tmp[i];
+  }
+  
+  n2 = find_nearest(codebook2, lspmelvq_cb[1].m, err, order);
+
+  for (i=0; i<order; i++) {
+    tmp[i] += codebook2[order*n2+i];
+    err[i] = x[i] - tmp[i];
+  }
+
+  n3 = find_nearest(codebook3, lspmelvq_cb[2].m, err, order);
+
+  for (i=0; i<order; i++) {
+    tmp[i] += codebook3[order*n3+i];
+  }
+  
+  for (i=0; i<order; i++) {
+      xq[i] = tmp[i];
+  }
+}
+
 #ifdef __EXPERIMENTAL__
 
 #define MBEST_STAGES 4
diff --git a/codec2-dev/src/quantise.h b/codec2-dev/src/quantise.h
index c9246c6b..ea4250fb 100644
--- a/codec2-dev/src/quantise.h
+++ b/codec2-dev/src/quantise.h
@@ -90,6 +90,7 @@ void lspjnd_quantise(float lsp[], float lsp_[], int order);
 void lspdt_quantise(float lsps[], float lsps_[], float lsps__prev[], int mode);
 void lspjvm_quantise(float lsps[], float lsps_[], int order);
 void lspanssi_quantise(float lsps[], float lsps_[], int order, int mbest_entries);
+void lspmelvq_quantise(float *x, float *xq, int order);
 
 void encode_mels_scalar(int mel_indexes[], float mels[], int order);
 void decode_mels_scalar(float mels[], int mel_indexes[], int order);
-- 
2.25.1