first version of kalman locally

Addresses #647.

analysis/statistics/a200a1cb296b4e0754792e2aa72d5af5940992c6.txt

@@ -0,0 +1,46 @@
+
+changeset: 1458:a200a1cb296b4e0754792e2aa72d5af5940992c6
+char kNewtonVersion[] = "0.3-alpha-1458 (a200a1cb296b4e0754792e2aa72d5af5940992c6) (build 03-15-2023-14:[email protected]_64)";
+\n./src/noisy/noisy-linux-EN -O0 applications/noisy/helloWorld.n -s
+\n./src/newton/newton-linux-EN -v 0 -eP applications/newton/invariants/ViolinWithTemperatureDependence-pigroups.nt
+
+Informational Report:
+---------------------
+Invariant "ViolinWithTemperatureDependenceForPiGroups" has 2 unique kernels, each with 2 column(s)...
+
+	Kernel 0 is a valid kernel:
+
+		   1   1
+		-0.5  -0
+		   1   0
+		 0.5   0
+		   0  -1
+		  -0  -1
+
+
+		The ordering of parameters is:	P1 P0 P3 P2 P4 P5 
+
+			Pi group 0, Pi 0 is:	P0^(-0.5)  P1^( 1)  P2^(0.5)  P3^( 1)  P4^( 0)  P5^(-0)  
+
+			Pi group 0, Pi 1 is:	P0^(-0)  P1^( 1)  P2^( 0)  P3^( 0)  P4^(-1)  P5^(-1)  
+
+
+	Kernel 1 is a valid kernel:
+
+		   1   0
+		-0.5   1
+		   1  -2
+		 0.5  -1
+		  -0  -2
+		   0  -2
+
+
+		The ordering of parameters is:	P1 P0 P3 P2 P4 P5 
+
+			Pi group 1, Pi 0 is:	P0^(-0.5)  P1^( 1)  P2^(0.5)  P3^( 1)  P4^(-0)  P5^( 0)  
+
+			Pi group 1, Pi 1 is:	P0^( 1)  P1^( 0)  P2^(-1)  P3^(-2)  P4^(-2)  P5^(-2)  
+
+
+
+

applications/newton/llvm-ir/c-files/kalman_filter.c

@@ -0,0 +1,3 @@
+//
+// Created by pei on 15/03/23.
+//

applications/newton/llvm-ir/kalman_filter.c

@@ -0,0 +1,66 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#define DT 0.01    // time step
+#define ACC_VAR 0.1 // acceleration measurement variance
+#define GYRO_VAR 0.1 // gyro measurement variance
+#define Q_ANGLE 0.1  // process noise covariance for the angle
+#define Q_BIAS 0.01  // process noise covariance for the gyro bias
+#define R_ANGLE 0.1  // measurement noise covariance for the angle
+
+typedef struct {
+    double angle;
+    double bias;
+    double rate;
+    double P[2][2];
+} kalman_state;
+
+// update the state of the Kalman filter based on a new measurement
+void kalman_update(kalman_state *state, double acc, double gyro) {
+    double y, S;
+    double K[2];
+
+    // update angle and bias estimates using the gyro measurement
+    state->angle += DT * (gyro - state->bias);
+    state->bias += Q_BIAS * DT;
+
+    // calculate the innovation and innovation covariance
+    y = acc - state->angle;
+    S = state->P[0][0] + R_ANGLE;
+
+    // calculate the Kalman gain
+    K[0] = state->P[0][0] / S;
+    K[1] = state->P[1][0] / S;
+
+    // update the state estimate and covariance
+    state->angle += K[0] * y;
+    state->bias += K[1] * y;
+    state->P[0][0] -= K[0] * state->P[0][0];
+    state->P[0][1] -= K[0] * state->P[0][1];
+    state->P[1][0] -= K[1] * state->P[0][0];
+    state->P[1][1] -= K[1] * state->P[0][1];
+
+    // propagate the state estimate and covariance using the gyro measurement
+    state->rate = gyro - state->bias;
+    state->angle += DT * state->rate;
+    state->P[0][0] += DT * (DT*state->P[1][1] - state->P[0][1] - state->P[1][0] + Q_ANGLE);
+    state->P[0][1] -= DT * state->P[1][1];
+    state->P[1][0] -= DT * state->P[1][1];
+    state->P[1][1] += Q_ANGLE * DT;
+}
+
+int main() {
+    double acc, gyro;
+    kalman_state state = {0, 0, 0, {{0, 0}, {0, 0}}};
+
+    // simulate some sensor measurements
+    for (int i = 0; i < 1000; i++) {
+        acc = 1.0 + ACC_VAR * (rand() / (double)RAND_MAX - 0.5);
+        gyro = 0.1 + GYRO_VAR * (rand() / (double)RAND_MAX - 0.5);
+        kalman_update(&state, acc, gyro);
+        printf("%f %f\n", state.angle, state.rate);
+    }
+
+    return 0;
+}