kalman added

PID.c

@@ -149,6 +149,7 @@ static double getspeed(const coordval_t *tagpos, PIDController_t *pid, axisdata_
                 // TODO: we can point somehow that we are at target or introduce new axis state
             }else DBG("Current abs error: %g", fe);
             break;
+        case AXIS_GONNASTOP:
         case AXIS_STOPPED: // start pointing to target; will change speed next time
             DBG("AXIS STOPPED!!!! --> Slewing");
             axis->state = AXIS_SLEWING;

kalman.c

@@ -0,0 +1,169 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <math.h>
+
+#include "kalman.h"
+
+void kalman3_init(Kalman3 *kf, double dt, double enc_var){
+    kf->dt = dt;
+
+    kf->x[0] = 0;
+    kf->x[1] = 0;
+    kf->x[2] = 0;
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++)
+            kf->P[i][j] = 0;
+
+    kf->P[0][0] = 1;
+    kf->P[1][1] = 1;
+    kf->P[2][2] = 1;
+
+    // process noise
+    kf->Q[0][0] = 1e-6;
+    kf->Q[1][1] = 1e-4;
+    kf->Q[2][2] = 1e-3;
+
+    kf->R = enc_var; // encoder noise variance
+}
+
+void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j){
+    double dt = kf->dt;
+
+    double dt2 = dt*dt;
+    double dt3 = dt2*dt;
+    double dt4 = dt3*dt;
+    double dt5 = dt4*dt;
+
+    double q = sigma_j * sigma_j;
+
+    kf->Q[0][0] = q * dt5 / 20.0;
+    kf->Q[0][1] = q * dt4 / 8.0;
+    kf->Q[0][2] = q * dt3 / 6.0;
+
+    kf->Q[1][0] = q * dt4 / 8.0;
+    kf->Q[1][1] = q * dt3 / 3.0;
+    kf->Q[1][2] = q * dt2 / 2.0;
+
+    kf->Q[2][0] = q * dt3 / 6.0;
+    kf->Q[2][1] = q * dt2 / 2.0;
+    kf->Q[2][2] = q * dt;
+}
+
+void kalman3_predict(Kalman3 *kf){
+    double dt = kf->dt;
+    double dt2 = 0.5 * dt * dt;
+
+    double theta = kf->x[0];
+    double omega = kf->x[1];
+    double alpha = kf->x[2];
+
+    // state prediction
+    kf->x[0] = theta + omega*dt + alpha*dt2;
+    kf->x[1] = omega + alpha*dt;
+    kf->x[2] = alpha;
+
+    // transition matrix
+    double F[3][3] =
+        {
+            {1, dt, dt2},
+            {0, 1,  dt},
+            {0, 0,  1}
+        };
+
+    // P = FPF^T + Q
+
+    double FP[3][3];
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++){
+            FP[i][j] =
+                F[i][0]*kf->P[0][j] +
+                F[i][1]*kf->P[1][j] +
+                F[i][2]*kf->P[2][j];
+        }
+
+    double Pnew[3][3];
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++){
+            Pnew[i][j] =
+                FP[i][0]*F[j][0] +
+                FP[i][1]*F[j][1] +
+                FP[i][2]*F[j][2] +
+                kf->Q[i][j];
+        }
+
+    for(int i=0;i<3;i++)
+        for(int j=0;j<3;j++)
+            kf->P[i][j] = Pnew[i][j];
+}
+
+void kalman3_update(Kalman3 *kf, double z){
+    // innovation
+    double y = z - kf->x[0];
+
+    // S = HPH^T + R
+    double S = kf->P[0][0] + kf->R;
+
+    // Kalman gain
+    double K[3];
+
+    K[0] = kf->P[0][0] / S;
+    K[1] = kf->P[1][0] / S;
+    K[2] = kf->P[2][0] / S;
+
+    // state update
+    kf->x[0] += K[0] * y;
+    kf->x[1] += K[1] * y;
+    kf->x[2] += K[2] * y;
+
+    // covariance update
+    double P00 = kf->P[0][0];
+    double P01 = kf->P[0][1];
+    double P02 = kf->P[0][2];
+
+    double P10 = kf->P[1][0];
+    double P11 = kf->P[1][1];
+    double P12 = kf->P[1][2];
+
+    double P20 = kf->P[2][0];
+    double P21 = kf->P[2][1];
+    double P22 = kf->P[2][2];
+
+    kf->P[0][0] = P00 - K[0]*P00;
+    kf->P[0][1] = P01 - K[0]*P01;
+    kf->P[0][2] = P02 - K[0]*P02;
+
+    kf->P[1][0] = P10 - K[1]*P00;
+    kf->P[1][1] = P11 - K[1]*P01;
+    kf->P[1][2] = P12 - K[1]*P02;
+
+    kf->P[2][0] = P20 - K[2]*P00;
+    kf->P[2][1] = P21 - K[2]*P01;
+    kf->P[2][2] = P22 - K[2]*P02;
+}
+
+
+// estimation of the R
+double encoder_noise(int counts){
+    double d = 2.0*M_PI / counts;
+    return d*d / 12.0;
+}
+

kalman.h

@@ -0,0 +1,34 @@
+/*
+ * This file is part of the libsidservo project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef struct{
+    double x[3];        // [theta, omega, alpha]
+    double P[3][3];     // covariance
+    double Q[3][3];     // process noise
+    double R;           // measurement noise
+    double dt;
+} Kalman3;
+
+
+double encoder_noise(int counts);
+void kalman3_update(Kalman3 *kf, double z);
+void kalman3_predict(Kalman3 *kf);
+void kalman3_set_jerk_noise(Kalman3 *kf, double sigma_j);
+void kalman3_init(Kalman3 *kf, double dt, double enc_var);

libsidservo.files

@@ -13,12 +13,14 @@ examples/dumpswing.c
 examples/goto.c
 examples/scmd_traectory.c
 examples/simpleconv.h
+kalman.c
 main.c
 sidservo.h
 serial.c
 examples/CMakeLists.txt
 examples/traectories.c
 examples/traectories.h
+kalman.h
 main.h
 movingmodel.c
 movingmodel.h

serial.c

@@ -31,6 +31,7 @@
 #include <sys/stat.h>
 #include <unistd.h>
 
+#include "kalman.h"
 #include "main.h"
 #include "movingmodel.h"
 #include "serial.h"
@@ -141,77 +142,6 @@ static void parse_encbuf(uint8_t databuf[ENC_DATALEN], struct timespec *t){
     //DBG("time = %zd+%zd/1e6, X=%g deg, Y=%g deg", tv->tv_sec, tv->tv_usec, mountdata.encposition.X*180./M_PI, mountdata.encposition.Y*180./M_PI);
 }
 
-#if 0
-/**
- * @brief getencval - get uint64_t data from encoder
- * @param fd - encoder fd
- * @param val - value read
- * @param t - measurement time
- * @return amount of data read or 0 if problem
- */
-static int getencval(int fd, double *val, struct timespec *t){
-    if(fd < 0){
-        DBG("Encoder fd < 0!");
-        return FALSE;
-    }
-    char buf[128];
-    int got = 0, Lmax = 127;
-    double t0 = timefromstart();
-    //DBG("start: %.6g", t0);
-    do{
-        fd_set rfds;
-        FD_ZERO(&rfds);
-        FD_SET(fd, &rfds);
-        struct timeval tv = encRtmout;
-        int retval = select(fd + 1, &rfds, NULL, NULL, &tv);
-        if(!retval){
-            //DBG("select()==0 - timeout, %.6g", timefromstart());
-            break;
-        }
-        if(retval < 0){
-            if(errno == EINTR){
-                DBG("EINTR");
-                continue;
-            }
-            DBG("select() < 0");
-            return 0;
-        }
-        if(FD_ISSET(fd, &rfds)){
-            ssize_t l = read(fd, &buf[got], Lmax);
-            if(l < 1){
-                DBG("read() < 0");
-                return 0; // disconnected ??
-            }
-            got += l; Lmax -= l;
-            buf[got] = 0;
-        } else continue;
-        if(buf[got-1] == '\n') break; // got EOL as last symbol
-    }while(Lmax && timefromstart() - t0 < Conf.EncoderReqInterval / 5.);
-    if(got == 0){
-        //DBG("No data from encoder, tfs=%.6g", timefromstart());
-        return 0;
-    }
-    char *estr = strrchr(buf, '\n');
-    if(!estr){
-        DBG("No EOL");
-        return 0;
-    }
-    *estr = 0;
-    char *bgn = strrchr(buf, '\n');
-    if(bgn) ++bgn;
-    else bgn = buf;
-    char *eptr;
-    long data = strtol(bgn, &eptr, 10);
-    if(eptr != estr){
-        DBG("NAN");
-        return 0; // wrong number
-    }
-    if(val) *val = (double) data;
-    if(t){ if(!curtime(t)){ DBG("Can't get time"); return 0; }}
-    return got;
-}
-#endif
-
 // try to read 1 byte from encoder; return -1 if nothing to read or -2 if device seems to be disconnected
 static int getencbyte(){
     if(encfd[0] < 0) return -1;
@@ -385,7 +315,7 @@ static int getdata(buf_t *buf, long *out){
     if(!buf || buf->len < 1 || buf->len > (XYBUFSZ+1)){
         return FALSE;
     }
-    DBG("got data");
+//    DBG("got data");
     // read record between last '\n' and previous (or start of string)
     char *last = &buf->buf[buf->len - 1];
     //DBG("buf: _%s_", buf->buf);
@@ -433,6 +363,18 @@ static void *encoderthread2(void _U_ *u){
     asknext(encfd[0]); asknext(encfd[1]);
     t0[0] = t0[1] = tstart = timefromstart();
     int errctr = 0;
+
+    // init Kalman for both axes
+    Kalman3 kf[2];
+    double dt = Conf.EncoderReqInterval; // 1ms encoders step
+    double sigma_jx = 1e-6, sigma_jy = 1e-6; // "jerk" sigma
+    double xnoice = encoder_noise(X_ENC_STEPSPERREV);
+    double ynoice = encoder_noise(Y_ENC_STEPSPERREV);
+    kalman3_init(&kf[0], dt, xnoice);
+    kalman3_init(&kf[1], dt, ynoice);
+    kalman3_set_jerk_noise(&kf[0], sigma_jx);
+    kalman3_set_jerk_noise(&kf[1], sigma_jy);
+
     do{ // main cycle
         if(poll(pfds, 2, 0) < 0){
             DBG("poll()");
@@ -450,25 +392,37 @@ static void *encoderthread2(void _U_ *u){
             double curt = timefromstart();
             if(getdata(&strbuf[i], &msrlast[i])) mtlast[i] = curt;
             if(curt - t0[i] >= Conf.EncoderReqInterval){ // get last records
-                DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]);
+                //DBG("last rec %d, curt=%g, t0=%g, mtlast=%g", i, curt, t0[i], mtlast[i]);
                 if(curt - mtlast[i] < 1.5*Conf.EncoderReqInterval){
-                    DBG("time OK");
+                    //DBG("time OK");
                     pthread_mutex_lock(&datamutex);
                     double pos = (double)msrlast[i];
-                    //DBG("pos[%d]=%g", i, pos);
-                    ;;
-                    //DBG("Got med: %g", pos);
                     if(i == 0){
-                        mountdata.encXposition.val = Xenc2rad(pos);
+                        pos = Xenc2rad(pos);
+                        // Kalman filtering
+                        kalman3_predict(&kf[i]);
+                        kalman3_update(&kf[i], pos);
+                        DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
+                            pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
+                        mountdata.encXposition.val = kf[i].x[0];
                         curtime(&mountdata.encXposition.t);
                         /*DBG("msrlast=%ld, Xpos.val=%g, t=%zd; XEzero=%d, SPR=%g",
                             msrlast[i], mountdata.encXposition.val, mountdata.encXposition.t.tv_sec,
                             X_ENC_ZERO, X_ENC_STEPSPERREV);*/
-                        getXspeed();
+                        //getXspeed();
+                        mountdata.encXspeed.val = kf[i].x[1];
+                        mountdata.encXspeed.t = mountdata.encXposition.t;
                     }else{
-                        mountdata.encYposition.val = Yenc2rad(pos);
+                        pos = Yenc2rad(pos);
+                        kalman3_predict(&kf[i]);
+                        kalman3_update(&kf[i], pos);
+                        DBG("Got pos=%g, kalman: angle=%g, vel=%g, acc=%g",
+                            pos, kf[i].x[0], kf[i].x[1], kf[i].x[2]);
+                        mountdata.encYposition.val = kf[i].x[0];
                         curtime(&mountdata.encYposition.t);
-                        getYspeed();
+                        //getYspeed();
+                        mountdata.encYspeed.val = kf[i].x[1];
+                        mountdata.encYspeed.t = mountdata.encYposition.t;
                     }
                     pthread_mutex_unlock(&datamutex);
                 }
@@ -860,11 +814,17 @@ static int bincmd(uint8_t *cmd, int len){
     }else{
         goto rtn;
     }
-    data_t d;
+    SSstat ans;
+    data_t d, in;
     d.buf = cmd;
     d.len = d.maxlen = len;
-    ret = wr(&d, NULL, 0);
+    in.buf = (uint8_t*)&ans; in.maxlen = sizeof(SSstat);
+    ret = wr(&d, &in, 0);
     DBG("%s", ret ? "SUCCESS" : "FAIL");
+    if(ret){
+        DBG("ANS: Xmot/Ymot: %d/%d, Ylast/Ylast: %d/%d",
+            ans.Xmot, ans.Ymot, ans.XLast, ans.YLast);
+    }
 rtn:
     pthread_mutex_unlock(&mntmutex);
     return ret;

sidservo.h

@@ -137,6 +137,7 @@ typedef struct{
 
 typedef enum{
     AXIS_STOPPED,   // stop
+    AXIS_GONNASTOP, // stop command run
     AXIS_SLEWING,   // go to target with maximal speed
     AXIS_POINTING,  // axis is in pointing zone, use PID
     AXIS_GUIDING,   // near target

ssii.c

@@ -45,27 +45,35 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
 }
 
 // Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
-static void chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t *stat){
+static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t stat){
     if(*prev == INT32_MAX){
-        *stat = AXIS_STOPPED;
+        stat = AXIS_STOPPED;
         DBG("START");
-    }else if(*stat != AXIS_STOPPED){
+    }else if(stat == AXIS_GONNASTOP){ // check stop
-        if(*prev == cur && ++(*nstopped) > MOTOR_STOPPED_CNT){
+        if(*prev == cur){
-            *stat = AXIS_STOPPED;
+            if(++(*nstopped) > MOTOR_STOPPED_CNT){
-            DBG("AXIS stopped");
+                stat = AXIS_STOPPED;
-        }
+                DBG("AXIS stopped");
+            }
+        }else *nstopped = 0;
     }else if(*prev != cur){
         DBG("AXIS moving");
         *nstopped = 0;
     }
     *prev = cur;
+    return stat;
 }
 // check for stopped/pointing states
 static void ChkStopped(const SSstat *s, mountdata_t *m){
     static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
     static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
-    chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, &m->Xstate);
+    axis_status_t Xstat, Ystat;
-    chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, &m->Ystate);
+    Xstat = chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, m->Xstate);
+    Ystat = chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, m->Ystate);
+    if(Xstat != m->Xstate || Ystat != m->Ystate){
+        DBG("Status changed");
+        setStat(Xstat, Ystat);
+    }
 }
 
 /**
@@ -170,14 +178,17 @@ int SSsetterI(const char *cmd, int32_t ival){
 }
 
 int SSstop(int emerg){
+    FNAME();
     int i = 0;
     const char *cmdx = (emerg) ? CMD_EMSTOPX : CMD_STOPX;
     const char *cmdy = (emerg) ? CMD_EMSTOPY : CMD_STOPY;
+    setStat(AXIS_GONNASTOP, AXIS_GONNASTOP);
     for(; i < 10; ++i){
         if(!SStextcmd(cmdx, NULL)) continue;
         if(SStextcmd(cmdy, NULL)) break;
     }
     if(i == 10) return FALSE;
+    DBG("Stopped");
     return TRUE;
 }