diff --git a/include/param_prj.h b/include/param_prj.h
index 3fdc0d8..79646eb 100644
--- a/include/param_prj.h
+++ b/include/param_prj.h
@@ -17,15 +17,15 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define VER 5.03.R
+#define VER 5.03.R.TIobs_test
 
 /* Entries must be ordered as follows:
    1. Saveable parameters (id != 0)
    2. Temporary parameters (id = 0)
    3. Display values
  */
-//Next param id (increase when adding new parameter!): 132
-//Next value Id: 2048
+//Next param id (increase when adding new parameter!): 135
+//Next value Id: 2050
 /*              category     name         unit       min     max     default id */
 
 #define MOTOR_PARAMETERS_COMMON \
@@ -33,6 +33,9 @@
     PARAM_ENTRY(CAT_MOTOR,   respolepairs,"",        1,      16,     1,      93  ) \
     PARAM_ENTRY(CAT_MOTOR,   sincosofs,   "dig",     1,      4096,   2048,   131 ) \
     PARAM_ENTRY(CAT_MOTOR,   encmode,     ENCMODES,  0,      5,      0,      75  ) \
+    PARAM_ENTRY(CAT_MOTOR,   anglemode,   ANGMODES,  0,      1,      0,      132 ) \
+    PARAM_ENTRY(CAT_MOTOR,   encK1,       "dig" ,    0,      100000, 550,    133 ) \
+    PARAM_ENTRY(CAT_MOTOR,   encK2,       "dig",     0,      100000, 80000,  134 ) \
     PARAM_ENTRY(CAT_MOTOR,   fmax,        "Hz",      21,     1000,   200,    9   ) \
     PARAM_ENTRY(CAT_MOTOR,   numimp,      "ppr",     8,      8192,   60,     15  ) \
     PARAM_ENTRY(CAT_MOTOR,   dirchrpm,    "rpm",     0,      20000,  100,    87  ) \
@@ -157,6 +160,8 @@
     VALUE_ENTRY(turns,       "",      2037 ) \
     VALUE_ENTRY(amp,         "dig",   2013 ) \
     VALUE_ENTRY(angle,       "°",     2014 ) \
+    VALUE_ENTRY(angleatan,   "°",     2048 ) \
+    VALUE_ENTRY(angleobs,    "°",     2049 ) \
     VALUE_ENTRY(pot,         "dig",   2015 ) \
     VALUE_ENTRY(pot2,        "dig",   2016 ) \
     VALUE_ENTRY(potnom,      "%",     2017 ) \
@@ -248,6 +253,7 @@
 #define OKERR        "0=Error, 1=Ok, 2=na"
 #define CHARGEMODS   "0=Off, 3=Boost, 4=Buck"
 #define ENCMODES     "0=Single, 1=AB, 2=ABZ, 3=SPI, 4=Resolver, 5=SinCos"
+#define ANGMODES     "0=atan2, 1=TIobs"
 #define POTMODES     "0=SingleRegen, 1=DualChannel, 2=CAN, 3=CANDual"
 #define CANSPEEDS    "0=250k, 1=500k, 2=800k, 3=1M"
 #define CANIOS       "1=Cruise, 2=Start, 4=Brake, 8=Fwd, 16=Rev, 32=Bms"
diff --git a/src/inc_encoder.cpp b/src/inc_encoder.cpp
index 17315e4..a212b68 100644
--- a/src/inc_encoder.cpp
+++ b/src/inc_encoder.cpp
@@ -41,6 +41,10 @@
 #define FRQ_TO_PSC(frq) ((72000000 / frq) - 1)
 #define NUM_ENCODER_CONFIGS (sizeof(encoderConfigurations) / sizeof(encoderConfigurations[0]))
 
+//TI encoder observer stuff
+static volatile int32_t integrator1 = 0;
+static volatile int32_t integrator2 = 0;
+
 typedef struct EncoderConfiguration
 {
    uint32_t pulseMeasFrequency;
@@ -249,7 +253,10 @@ void Encoder::UpdateRotorFrequency(int callingFrequency)
       int absTurns = ABS(turnsSinceLastSample);
       if (startupDelay == 0 && absTurns > STABLE_ANGLE)
       {
-         lastFrequency = (callingFrequency * absTurns) / FP_TOINT(TWO_PI);
+         if (encMode == SPI)
+         {
+            lastFrequency = (callingFrequency * absTurns) / FP_TOINT(TWO_PI);
+         }
          detectedDirection = turnsSinceLastSample > 0 ? 1 : -1;
       }
       else
@@ -556,8 +563,36 @@ uint16_t Encoder::DecodeAngle(bool invert)
    if ((resolverMax - resolverMin) > MIN_RES_AMP)
    {
       if (invert)
-         return SineCore::Atan2(-sin, -cos);
-      return SineCore::Atan2(sin, cos);
+      {
+         sin = -sin;
+         cos = -cos;
+      }
+      uint16_t angleatan = SineCore::Atan2(sin, cos);
+      Param::SetFlt(Param::angleatan, FP_FROMINT(angleatan) / (65536 / 360)); //original atan2 angle calculation
+
+      //TI observer based implementation
+      uint16_t K1 = Param::GetInt(Param::encK1); //probably doesn't need to be a parameter.
+      uint16_t K2 = Param::GetInt(Param::encK2); //likewise
+      uint16_t frequency = 8000;// hz. Could be set dynamically if needed, but seems to work ok static.
+      int32_t cos_sin_anglast = cos*SineCore::Sine(integrator2)/32767; //angle_last = integrator2
+      int32_t sin_cos_anglast = sin*SineCore::Cosine(integrator2)/32767;
+      int32_t sum_one = sin_cos_anglast - cos_sin_anglast;
+      integrator1 += sum_one*K2/frequency;
+      int32_t gain1 = sum_one*K1;
+      int32_t sum_two = integrator1 + gain1;
+      integrator2 += sum_two/frequency;
+      int32_t sample_delay_comp = integrator1/(2*frequency);
+      uint16_t obs_angle = sample_delay_comp-integrator2+16384; //digits, 2pi rad = 360 deg = 65536 digits. Offset and -ve to match original atan2 code - prevents need to reset encoffset
+      Param::SetFlt(Param::angleobs, FP_FROMINT(obs_angle) / (65536 / 360));
+      lastFrequency = ABS(FP_FROMINT(integrator1)/TWO_PI);
+      if (Param::GetInt(Param::anglemode) == 1) //TI observer mode
+      {
+         return obs_angle;
+      }
+      else //original atan2 mode
+      {
+         return angleatan;
+      }
    }
    else
    {